1448 lines
48 KiB
C
1448 lines
48 KiB
C
#include "ptz_header_file.h"
|
||
#include <math.h>
|
||
#include "app_cfg.h"
|
||
#include "gd32f4xx.h"
|
||
#include "device_dac_out.h"
|
||
|
||
|
||
#ifdef PTZ_BLDC_MOTOR
|
||
|
||
//发送云台实际转速
|
||
void ptz_send_speed(char dev, char speed)
|
||
{
|
||
unsigned char Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
|
||
unsigned char Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
|
||
unsigned short int HSpeed = 0;
|
||
unsigned short int VSpeed = 0;
|
||
Speed1[1] = g_ptz.address;
|
||
Speed2[1] = g_ptz.address;
|
||
|
||
HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5);
|
||
Speed1[3] = 0X03;//水平电机
|
||
Speed1[4] = (unsigned char)(HSpeed >> 8);
|
||
Speed1[5] = (unsigned char)(HSpeed & 0x00ff);
|
||
Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1));
|
||
|
||
VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5);
|
||
Speed2[3] = 0X04;//垂直电机
|
||
Speed2[4] = (unsigned char)(VSpeed >> 8);
|
||
Speed2[5] = (unsigned char)(VSpeed & 0x00ff);
|
||
Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2));
|
||
|
||
switch(speed)
|
||
{
|
||
case 1://只查询水平转速
|
||
ptz_send_data(dev,Speed1,sizeof(Speed1));
|
||
|
||
break;
|
||
|
||
case 2://只查询垂直转速
|
||
ptz_send_data(dev,Speed2,sizeof(Speed2));
|
||
|
||
break;
|
||
|
||
default://水平转速垂直转速都查询
|
||
ptz_send_data(dev,Speed1,sizeof(Speed1));
|
||
ptz_send_data(dev,Speed2,sizeof(Speed2));
|
||
|
||
break;
|
||
}
|
||
}
|
||
|
||
|
||
void ptz_pid_init()
|
||
{
|
||
g_ptz.hori_pid.SumError = 0;//累计误差
|
||
g_ptz.hori_pid.LastError = 0;//上一次输入偏差
|
||
g_ptz.hori_pid.PrevError = 0;//上上次输入偏差
|
||
g_ptz.hori_pid.LastUT_float = 0;//上次PID输出值u(t)
|
||
g_ptz.hori_pid.PidUT_float = 0;//PID当前输出值
|
||
g_ptz.hori_pid.KP = PTZ_HORI_PID_HORI_KP;//比例常数
|
||
g_ptz.hori_pid.TI = PTZ_HORI_PID_HORI_TI;//积分常数
|
||
g_ptz.hori_pid.TD = PTZ_HORI_PID_HORI_TD;//微分常数
|
||
g_ptz.hori_pid.T = PTZ_HORI_PID_T / 1000.0;//采样周期
|
||
//三个简化参数A，B，C
|
||
g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD);
|
||
g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
|
||
g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T);
|
||
|
||
g_ptz.vert_pid.SumError = 0;//累计误差
|
||
g_ptz.vert_pid.LastError = 0;//上一次输入偏差
|
||
g_ptz.vert_pid.PrevError = 0;//上上次输入偏差
|
||
g_ptz.vert_pid.LastUT_float = 0;//上次PID输出值u(t)
|
||
g_ptz.vert_pid.PidUT_float = 0;//PID当前输出值
|
||
g_ptz.vert_pid.KP = PTZ_VERT_PID_VERT_KP;//比例常数
|
||
g_ptz.vert_pid.TI = PTZ_VERT_PID_VERT_TI;//积分常数
|
||
g_ptz.vert_pid.TD = PTZ_VERT_PID_VERT_TD;//微分常数
|
||
g_ptz.vert_pid.T = PTZ_VERT_PID_T / 1000.0;//采样周期
|
||
//三个简化参数A，B，C
|
||
g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD);
|
||
g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
|
||
g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
|
||
}
|
||
|
||
void ptz_hori_pid_clear_zero()
|
||
{
|
||
g_ptz.hori_pid.SumError = 0;//累计误差
|
||
g_ptz.hori_pid.LastError = 0;//上一次输入偏差
|
||
g_ptz.hori_pid.PrevError = 0;//上上次输入偏差
|
||
g_ptz.hori_pid.LastUT_float = 0;//上次PID输出值u(t)
|
||
g_ptz.hori_pid.PidUT_float = 0;//PID当前输出值
|
||
g_ptz.hori_pid.PidUT_uint = 0;
|
||
hori_dac0_data_out(0);
|
||
}
|
||
|
||
static float ptz_hori_pid_calculate(float H_SampSpeed)
|
||
{
|
||
float H_IError,H_IIncPid;
|
||
H_IError = g_ptz.hori_speed_set - H_SampSpeed;//当前误差PID_INPUT_LIMIT
|
||
/*控制PID输入参数*/
|
||
// if(H_IError > PTZ_HORI_PID_INPUT_LIMIT)
|
||
// {
|
||
// H_IError = PTZ_HORI_PID_INPUT_LIMIT;
|
||
// }
|
||
// if(H_IError < (-1 * PTZ_HORI_PID_INPUT_LIMIT))
|
||
// {
|
||
// H_IError = PTZ_HORI_PID_INPUT_LIMIT * -1;
|
||
// }
|
||
//增量计算
|
||
H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError;
|
||
//存储误差，用于下次计算
|
||
g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
|
||
g_ptz.hori_pid.LastError = H_IError;
|
||
//返回增量值
|
||
return H_IIncPid;
|
||
}
|
||
|
||
|
||
|
||
//新调速方式
|
||
static void ptz_hori_pid_task()
|
||
{
|
||
unsigned int time = 0;
|
||
char i = 0;
|
||
while(1)
|
||
{
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
#ifdef PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED
|
||
//磁编码器测速，数据清0
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_a = 0;
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_b = 0;
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c = 0;
|
||
|
||
//磁编码器测速，测速起始角度
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_a = g_ptz.hori_as5048a.as5048a_angle_actual;
|
||
|
||
//PID测速延时
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
|
||
|
||
|
||
//磁编码器测速，测速结束角度
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_b = g_ptz.hori_as5048a.as5048a_angle_actual;
|
||
//根据磁编码器转动的方向
|
||
//由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态
|
||
//反转，云台角度减小，磁编码角度增大
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
if(g_ptz.hori_as5048a.as5048a_speed_angle_a <= g_ptz.hori_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_b - g_ptz.hori_as5048a.as5048a_speed_angle_a;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.hori_as5048a.as5048a_speed_angle_a) +
|
||
(g_ptz.hori_as5048a.as5048a_speed_angle_b - 0);
|
||
}
|
||
}
|
||
//正转，云台角度增大，磁编码角度减小
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
if(g_ptz.hori_as5048a.as5048a_speed_angle_a >= g_ptz.hori_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_a - g_ptz.hori_as5048a.as5048a_speed_angle_b;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.hori_as5048a.as5048a_speed_angle_b) +
|
||
(g_ptz.hori_as5048a.as5048a_speed_angle_a - 0);
|
||
}
|
||
}
|
||
|
||
//去除转角异常情况
|
||
if(g_ptz.hori_as5048a.as5048a_speed_angle_c >= 50.0 ||
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c < 0)
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c = g_ptz.hori_as5048a.as5048a_speed_angle_c_last;
|
||
}
|
||
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c_last = g_ptz.hori_as5048a.as5048a_speed_angle_c;
|
||
|
||
//计算磁编码器转速
|
||
g_ptz.hori_as5048a.as5048a_speed_actual = (g_ptz.hori_as5048a.as5048a_speed_angle_c * 1000.0) /
|
||
(360.0 * PTZ_HORI_PID_T) * 60.0;
|
||
|
||
//根据磁编码器计算电机转速
|
||
g_ptz.hori_motor_speed_as5048a_actual =
|
||
g_ptz.hori_as5048a.as5048a_speed_actual * PTZ_HORI_MOTOR_RATIO;
|
||
g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_as5048a_actual;
|
||
//根据磁编码器计算云台转速
|
||
g_ptz.hori_speed_as5048a_actual =
|
||
g_ptz.hori_as5048a.as5048a_speed_actual / PTZ_HORI_BIG_GEAR_RATIO;
|
||
|
||
g_ptz.hori_speed_actual = g_ptz.hori_speed_as5048a_actual;
|
||
|
||
#endif
|
||
|
||
#ifdef PTZ_HORI_PID_AS5048A_ANGLE_SYN_SPEED
|
||
//磁编码器测速，数据清0
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_a = 0;
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_b = 0;
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c = 0;
|
||
|
||
//磁编码器测速，测速起始角度
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_a = g_ptz.hori_as5048a.as5048a_angle_actual;
|
||
|
||
//PID测速延时
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
|
||
|
||
|
||
//磁编码器测速，测速结束角度
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_b = g_ptz.hori_as5048a.as5048a_angle_actual;
|
||
//根据磁编码器转动的方向
|
||
//由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态
|
||
//正转，云台角度增大，磁编码角度增大
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
if(g_ptz.hori_as5048a.as5048a_speed_angle_a <= g_ptz.hori_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_b - g_ptz.hori_as5048a.as5048a_speed_angle_a;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.hori_as5048a.as5048a_speed_angle_a) +
|
||
(g_ptz.hori_as5048a.as5048a_speed_angle_b - 0);
|
||
}
|
||
}
|
||
//反转，云台角度减小，磁编码角度减小
|
||
if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
if(g_ptz.hori_as5048a.as5048a_speed_angle_a >= g_ptz.hori_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_a - g_ptz.hori_as5048a.as5048a_speed_angle_b;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.hori_as5048a.as5048a_speed_angle_b) +
|
||
(g_ptz.hori_as5048a.as5048a_speed_angle_a - 0);
|
||
}
|
||
}
|
||
|
||
//去除转角异常情况
|
||
if(g_ptz.hori_as5048a.as5048a_speed_angle_c >= 50.0 ||
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c < 0)
|
||
{
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c = g_ptz.hori_as5048a.as5048a_speed_angle_c_last;
|
||
}
|
||
|
||
g_ptz.hori_as5048a.as5048a_speed_angle_c_last = g_ptz.hori_as5048a.as5048a_speed_angle_c;
|
||
|
||
//计算磁编码器转速
|
||
g_ptz.hori_as5048a.as5048a_speed_actual = (g_ptz.hori_as5048a.as5048a_speed_angle_c * 1000.0) /
|
||
(360.0 * PTZ_HORI_PID_T) * 60.0;
|
||
|
||
//根据磁编码器计算电机转速
|
||
g_ptz.hori_motor_speed_as5048a_actual =
|
||
g_ptz.hori_as5048a.as5048a_speed_actual * PTZ_HORI_MOTOR_RATIO;
|
||
g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_as5048a_actual;
|
||
//根据磁编码器计算云台转速
|
||
g_ptz.hori_speed_as5048a_actual =
|
||
g_ptz.hori_as5048a.as5048a_speed_actual / PTZ_HORI_BIG_GEAR_RATIO;
|
||
|
||
g_ptz.hori_speed_actual = g_ptz.hori_speed_as5048a_actual;
|
||
#endif
|
||
|
||
|
||
#ifdef PTZ_HORI_PID_JY02A_SPEED
|
||
//电机驱动芯片测速,数据清0
|
||
g_ptz.hori_jy02a.jy02a_fg_num_last = g_ptz.hori_jy02a.jy02a_fg_num_actual;
|
||
g_ptz.hori_jy02a.jy02a_fg_num_actual = 0;
|
||
g_ptz.hori_motor_speed_actual = 0;
|
||
|
||
//PID测速延时
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
|
||
|
||
//根据电机驱动芯片计算电机实际转速
|
||
g_ptz.hori_motor_speed_jy02a_actual =
|
||
g_ptz.hori_jy02a.jy02a_fg_num_actual /
|
||
PTZ_HORI_FG_PWM_NUM_CYCLE *
|
||
1000.0 * 60 / PTZ_HORI_PID_T;
|
||
g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_jy02a_actual;
|
||
//根据电机驱动芯片计算云台转速
|
||
g_ptz.hori_speed_jy02a_actual = g_ptz.hori_motor_speed_jy02a_actual / PTZ_HORI_RATIO;
|
||
|
||
g_ptz.hori_speed_actual = g_ptz.hori_speed_jy02a_actual;
|
||
#endif
|
||
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
|
||
g_ptz.hori_pid.hall_h1_count = 0;
|
||
g_ptz.hori_pid.hall_h2_count = 0;
|
||
g_ptz.hori_pid.hall_h3_count = 0;
|
||
g_ptz.hori_pid.hall_h123_count = 0;
|
||
|
||
g_ptz.hori_pid.hall_h1_count_last = 0;
|
||
g_ptz.hori_pid.hall_h1_count_time = 0;
|
||
g_ptz.hori_pid.hall_h1_count_time_s = 0;
|
||
g_ptz.hori_pid.hall_h1_count_time_e = 0;
|
||
|
||
g_ptz.hori_pid.hall_h2_count_last = 0;
|
||
g_ptz.hori_pid.hall_h2_count_time = 0;
|
||
g_ptz.hori_pid.hall_h2_count_time_s = 0;
|
||
g_ptz.hori_pid.hall_h2_count_time_e = 0;
|
||
|
||
g_ptz.hori_pid.hall_h3_count_last = 0;
|
||
g_ptz.hori_pid.hall_h3_count_time = 0;
|
||
g_ptz.hori_pid.hall_h3_count_time_s = 0;
|
||
g_ptz.hori_pid.hall_h3_count_time_e = 0;
|
||
|
||
|
||
g_ptz.hori_pid.hall_h123_count_last = 0;
|
||
g_ptz.hori_pid.hall_h123_count_time = 0;
|
||
g_ptz.hori_pid.hall_h123_count_time_s = 0;
|
||
g_ptz.hori_pid.hall_h123_count_time_e = 0;
|
||
|
||
g_ptz.hori_pid.hall_h1_start_flag = 0;
|
||
g_ptz.hori_pid.hall_h2_start_flag = 0;
|
||
g_ptz.hori_pid.hall_h3_start_flag = 0;
|
||
g_ptz.hori_pid.hall_h123_start_flag = 0;
|
||
|
||
for(time = 0; time < PTZ_HORI_PID_T_MAX; time ++)
|
||
{
|
||
if(g_ptz.hori_pid.hall_h1_count != g_ptz.hori_pid.hall_h1_count_last)
|
||
{
|
||
if(g_ptz.hori_pid.hall_h1_start_flag == 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h1_count_last = g_ptz.hori_pid.hall_h1_count;
|
||
g_ptz.hori_pid.hall_h1_count_time_s = time;
|
||
g_ptz.hori_pid.hall_h1_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h1_count_last = g_ptz.hori_pid.hall_h1_count;
|
||
g_ptz.hori_pid.hall_h1_count_time_e = time;
|
||
}
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.hori_pid.hall_h123_start_flag == 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time_s = time;
|
||
g_ptz.hori_pid.hall_h123_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time_e = time;
|
||
}
|
||
}
|
||
|
||
if(g_ptz.hori_pid.hall_h2_count != g_ptz.hori_pid.hall_h2_count_last)
|
||
{
|
||
if(g_ptz.hori_pid.hall_h2_start_flag == 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h2_count_last = g_ptz.hori_pid.hall_h2_count;
|
||
g_ptz.hori_pid.hall_h2_count_time_s = time;
|
||
g_ptz.hori_pid.hall_h2_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h2_count_last = g_ptz.hori_pid.hall_h2_count;
|
||
g_ptz.hori_pid.hall_h2_count_time_e = time;
|
||
}
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.hori_pid.hall_h123_start_flag == 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time_s = time;
|
||
g_ptz.hori_pid.hall_h123_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time_e = time;
|
||
}
|
||
}
|
||
|
||
if(g_ptz.hori_pid.hall_h3_count != g_ptz.hori_pid.hall_h3_count_last)
|
||
{
|
||
if(g_ptz.hori_pid.hall_h3_start_flag == 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h3_count_last = g_ptz.hori_pid.hall_h3_count;
|
||
g_ptz.hori_pid.hall_h3_count_time_s = time;
|
||
g_ptz.hori_pid.hall_h3_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h3_count_last = g_ptz.hori_pid.hall_h3_count;
|
||
g_ptz.hori_pid.hall_h3_count_time_e = time;
|
||
}
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.hori_pid.hall_h123_start_flag == 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time_s = time;
|
||
g_ptz.hori_pid.hall_h123_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time_e = time;
|
||
}
|
||
}
|
||
|
||
if(time >= PTZ_HORI_PID_T)//到达默认采集周期
|
||
{
|
||
if(g_ptz.hori_pid.hall_h1_count >= 2 &&
|
||
g_ptz.hori_pid.hall_h2_count >= 2 &&
|
||
g_ptz.hori_pid.hall_h3_count >= 2)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
|
||
|
||
|
||
|
||
//计算单独时间
|
||
if(g_ptz.hori_pid.hall_h1_count_time_e > g_ptz.hori_pid.hall_h1_count_time_s)
|
||
{
|
||
g_ptz.hori_pid.hall_h1_count_time =
|
||
g_ptz.hori_pid.hall_h1_count_time_e - g_ptz.hori_pid.hall_h1_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h1_count_time = time;
|
||
}
|
||
|
||
if(g_ptz.hori_pid.hall_h2_count_time_e > g_ptz.hori_pid.hall_h2_count_time_s)
|
||
{
|
||
g_ptz.hori_pid.hall_h2_count_time =
|
||
g_ptz.hori_pid.hall_h2_count_time_e - g_ptz.hori_pid.hall_h2_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h2_count_time = time;
|
||
}
|
||
|
||
if(g_ptz.hori_pid.hall_h3_count_time_e > g_ptz.hori_pid.hall_h3_count_time_s)
|
||
{
|
||
g_ptz.hori_pid.hall_h3_count_time =
|
||
g_ptz.hori_pid.hall_h3_count_time_e - g_ptz.hori_pid.hall_h3_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h3_count_time = time;
|
||
}
|
||
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.hori_pid.hall_h123_count_time_e > g_ptz.hori_pid.hall_h123_count_time_s)
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time =
|
||
g_ptz.hori_pid.hall_h123_count_time_e - g_ptz.hori_pid.hall_h123_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h123_count_time = time;
|
||
}
|
||
|
||
//根据三个霍尔的反馈情况分别计算转速
|
||
i = 0;
|
||
if(g_ptz.hori_pid.hall_h1_count >= 2 && g_ptz.hori_pid.hall_h1_count_time > 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h1_motor_speed =
|
||
60000.0 * (float)(g_ptz.hori_pid.hall_h1_count - 1) /
|
||
(float)g_ptz.hori_pid.hall_h1_count_time /
|
||
PTZ_HORI_MOTOR_POLE_PAIRS;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h1_motor_speed = 0;
|
||
}
|
||
|
||
|
||
if(g_ptz.hori_pid.hall_h2_count >= 2 && g_ptz.hori_pid.hall_h2_count_time > 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h2_motor_speed =
|
||
60000.0 * (float)(g_ptz.hori_pid.hall_h2_count - 1) /
|
||
(float)g_ptz.hori_pid.hall_h2_count_time /
|
||
PTZ_HORI_MOTOR_POLE_PAIRS;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h2_motor_speed = 0;
|
||
}
|
||
|
||
|
||
if(g_ptz.hori_pid.hall_h3_count >= 2 && g_ptz.hori_pid.hall_h3_count_time > 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h3_motor_speed =
|
||
60000.0 * (float)(g_ptz.hori_pid.hall_h3_count - 1) /
|
||
(float)g_ptz.hori_pid.hall_h3_count_time /
|
||
PTZ_HORI_MOTOR_POLE_PAIRS;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h3_motor_speed = 0;
|
||
}
|
||
|
||
g_ptz.hori_pid.hall_h123_count = g_ptz.hori_pid.hall_h1_count +
|
||
g_ptz.hori_pid.hall_h2_count +
|
||
g_ptz.hori_pid.hall_h3_count;
|
||
//根据霍尔总脉冲计算电机转速
|
||
if(g_ptz.hori_pid.hall_h123_count >= 2 && g_ptz.hori_pid.hall_h123_count_time > 0)
|
||
{
|
||
g_ptz.hori_pid.hall_h123_motor_speed =
|
||
60000.0 * (float)(g_ptz.hori_pid.hall_h123_count - 1) /
|
||
(float)g_ptz.hori_pid.hall_h123_count_time /
|
||
PTZ_HORI_ONE_CYCLE_HALL_NUM;
|
||
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.hall_h123_motor_speed = 0;
|
||
}
|
||
|
||
|
||
//计算电机平均转速
|
||
if(i > 0)
|
||
{
|
||
g_ptz.hori_motor_speed_hall_actual = (g_ptz.hori_pid.hall_h1_motor_speed +
|
||
g_ptz.hori_pid.hall_h2_motor_speed +
|
||
g_ptz.hori_pid.hall_h3_motor_speed +
|
||
g_ptz.hori_pid.hall_h123_motor_speed) / i;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_motor_speed_hall_actual = 0;
|
||
}
|
||
|
||
|
||
//判断电机转动是否抖动
|
||
if(g_ptz.hori_pid.hall_h1_count == 0 ||
|
||
g_ptz.hori_pid.hall_h2_count == 0 ||
|
||
g_ptz.hori_pid.hall_h3_count == 0)
|
||
{//电机在抖动，速度直接置0
|
||
g_ptz.hori_motor_speed_hall_actual = 0;
|
||
}
|
||
|
||
|
||
g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_hall_actual;
|
||
|
||
//根据电机转速计算云台转速
|
||
g_ptz.hori_speed_hall_actual = g_ptz.hori_motor_speed_hall_actual / PTZ_HORI_RATIO;
|
||
g_ptz.hori_speed_actual = g_ptz.hori_speed_hall_actual;
|
||
#endif
|
||
|
||
|
||
|
||
/*由于系统实时调节能力不足将调速分为两个部分：
|
||
1.启动时首先进入直线比例快速提速；
|
||
2.速度达到设定速度的60％后转换为PID调速。*/
|
||
|
||
|
||
if(g_ptz.hori_speed_actual < g_ptz.hori_speed_set * PTZ_HORI_PRO_ADD_RANGE_LESS)
|
||
{
|
||
// if(time > PTZ_HORI_PID_ADD)
|
||
// {
|
||
// time = PTZ_HORI_PID_ADD;
|
||
// }
|
||
// g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float + time/*PTZ_HORI_PID_ADD*/;
|
||
// //存储误差，用于PID计算
|
||
// g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
|
||
// g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual;
|
||
|
||
g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * PTZ_HORI_PRO_ADD_MUL_LESS;//* 1.3/*PTZ_HORI_PID_ADD*/;
|
||
if(g_ptz.hori_pid.PidUT_float < PTZ_HORI_PID_ADD)
|
||
{
|
||
g_ptz.hori_pid.PidUT_float = PTZ_HORI_PID_ADD;
|
||
}
|
||
//存储误差，用于PID计算
|
||
g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
|
||
g_ptz.hori_pid.LastError = g_ptz.hori_speed_set - g_ptz.hori_speed_actual;
|
||
/**考虑超调时的直线降速，否则超调后降速会很慢，可以比列减速**/
|
||
}
|
||
else
|
||
{
|
||
//计算PID控制器输出值
|
||
g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float;
|
||
|
||
//控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
|
||
//即防止PID增量过大
|
||
if(fabs(g_ptz.hori_pid.PidUT_float - g_ptz.hori_pid.LastUT_float) > PTZ_HORI_PID_OUTPUT_LIMIT)//限定PID输出限定
|
||
{
|
||
if(g_ptz.hori_pid.PidUT_float > g_ptz.hori_pid.LastUT_float)
|
||
{
|
||
g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float + PTZ_HORI_PID_OUTPUT_LIMIT;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float - PTZ_HORI_PID_OUTPUT_LIMIT;
|
||
}
|
||
}
|
||
}
|
||
|
||
if(g_ptz.hori_pid.PidUT_float < PTZ_HORI_VR_MIN)//限制输入模拟电压最小值
|
||
{
|
||
g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MIN;
|
||
}
|
||
if(g_ptz.hori_pid.PidUT_float > PTZ_HORI_VR_MAX)//限制输入模拟电压最大值
|
||
{
|
||
g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MAX;
|
||
}
|
||
|
||
g_ptz.hori_pid.PidUT_uint = (unsigned int)(g_ptz.hori_pid.PidUT_float + 0.5);
|
||
|
||
//限制PID的输出值在某个指定的范围
|
||
if(g_ptz.hori_pid.PidUT_uint <= PTZ_HORI_VR_MIN)//限制输入模拟电压最小值
|
||
{
|
||
g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MIN;
|
||
}
|
||
if(g_ptz.hori_pid.PidUT_uint > PTZ_HORI_VR_MAX)//限制输入模拟电压最大值
|
||
{
|
||
g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MAX;
|
||
}
|
||
|
||
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
|
||
hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint);
|
||
|
||
//将当前PID输出值保存
|
||
g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float;
|
||
}
|
||
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
||
{
|
||
g_ptz.hori_speed_actual = 0;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
}
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
|
||
//防止数据超限溢出
|
||
if(g_ptz.hori_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.hori_pid.hall_h1_count = 0;
|
||
}
|
||
if(g_ptz.hori_pid.hall_h2_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.hori_pid.hall_h2_count = 0;
|
||
}
|
||
if(g_ptz.hori_pid.hall_h3_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.hori_pid.hall_h3_count = 0;
|
||
}
|
||
#endif
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
static OS_STK task_hori_pid_stk[TASK_HORI_PID_STK_SIZE];
|
||
static void creat_task_hori_pid(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_pid_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_hori_pid_stk[TASK_HORI_PID_STK_SIZE - 1],
|
||
(INT8U ) TASK_HORI_PID_PRIO,
|
||
(INT16U ) TASK_HORI_PID_PRIO,
|
||
(OS_STK *)&task_hori_pid_stk[0],
|
||
(INT32U ) TASK_HORI_PID_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_pid_task", &name_err);
|
||
#endif
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
/****************************************************************************/
|
||
|
||
void ptz_vert_pid_clear_zero()
|
||
{
|
||
g_ptz.vert_pid.SumError = 0;//累计误差
|
||
g_ptz.vert_pid.LastError = 0;//上一次输入偏差
|
||
g_ptz.vert_pid.PrevError = 0;//上上次输入偏差
|
||
g_ptz.vert_pid.LastUT_float = 0;//上次PID输出值u(t)
|
||
g_ptz.vert_pid.PidUT_float = 0;//PID当前输出值
|
||
g_ptz.vert_pid.PidUT_uint = 0;
|
||
g_ptz.vert_pid.mode = PID_ADJUST_SPEED;//首先默认PID调速
|
||
g_ptz.vert_pid.director_speed_state = 0;
|
||
vert_dac1_data_out(0);
|
||
}
|
||
|
||
static float ptz_vert_pid_calculate(float SampSpeed)
|
||
{
|
||
float IError,IIncPid;
|
||
IError = g_ptz.vert_speed_set - SampSpeed;//当前误差PID_INPUT_LIMIT
|
||
|
||
/*控制PID输入参数*/
|
||
// if(IError > PTZ_VERT_PID_INPUT_LIMIT)
|
||
// {
|
||
// IError = PTZ_VERT_PID_INPUT_LIMIT;
|
||
// }
|
||
// if(IError < (-1 * PTZ_VERT_PID_INPUT_LIMIT))
|
||
// {
|
||
// IError = PTZ_VERT_PID_INPUT_LIMIT * -1;
|
||
// }
|
||
//增量计算
|
||
IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError;
|
||
//存储误差，用于下次计算
|
||
g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
|
||
g_ptz.vert_pid.LastError = IError;
|
||
//返回增量值
|
||
return IIncPid;
|
||
}
|
||
|
||
|
||
|
||
//新调速方式
|
||
static void ptz_vert_pid_task()
|
||
{
|
||
unsigned int time = 0;
|
||
char i = 0;
|
||
while(1)
|
||
{
|
||
//计算转速
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
{
|
||
#ifdef PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED
|
||
//磁编码器测速，数据清0
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_a = 0;
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_b = 0;
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c = 0;
|
||
|
||
//磁编码器测速，测速起始角度
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_a = g_ptz.vert_as5048a.as5048a_angle_actual;
|
||
|
||
//PID测速延时
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
|
||
|
||
//磁编码器测速，测速结束角度
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_b = g_ptz.vert_as5048a.as5048a_angle_actual;
|
||
|
||
//根据磁编码器转动的方向
|
||
//由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态
|
||
//反转，云台角度减小，磁编码角度增大
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
if(g_ptz.vert_as5048a.as5048a_speed_angle_a <= g_ptz.vert_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_b - g_ptz.vert_as5048a.as5048a_speed_angle_a;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.vert_as5048a.as5048a_speed_angle_a) +
|
||
(g_ptz.vert_as5048a.as5048a_speed_angle_b - 0);
|
||
}
|
||
}
|
||
//正转，云台角度增大，磁编码角度减小
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
if(g_ptz.vert_as5048a.as5048a_speed_angle_a >= g_ptz.vert_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_a - g_ptz.vert_as5048a.as5048a_speed_angle_b;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.vert_as5048a.as5048a_speed_angle_b) +
|
||
(g_ptz.vert_as5048a.as5048a_speed_angle_a - 0);
|
||
}
|
||
}
|
||
|
||
//去除转角异常情况
|
||
if(g_ptz.vert_as5048a.as5048a_speed_angle_c < 0 ||
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c >= 50.0)
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c = g_ptz.vert_as5048a.as5048a_speed_angle_c_last;
|
||
}
|
||
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c_last = g_ptz.vert_as5048a.as5048a_speed_angle_c;
|
||
|
||
//计算磁编码器转速
|
||
g_ptz.vert_as5048a.as5048a_speed_actual = (g_ptz.vert_as5048a.as5048a_speed_angle_c * 1000) /
|
||
(360.0 * PTZ_VERT_PID_T) * 60.0;
|
||
//根据磁编码器计算电机转速
|
||
g_ptz.vert_motor_speed_as5048a_actual =
|
||
g_ptz.vert_as5048a.as5048a_speed_actual * PTZ_VERT_MOTOR_RATIO;
|
||
g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_as5048a_actual;
|
||
//根据磁编码器计算云台转速
|
||
g_ptz.vert_speed_as5048a_actual =
|
||
g_ptz.vert_as5048a.as5048a_speed_actual / PTZ_VERT_BIG_GEAR_RATIO;
|
||
|
||
g_ptz.vert_speed_actual = g_ptz.vert_speed_as5048a_actual;
|
||
#endif
|
||
|
||
#ifdef PTZ_VERT_PID_AS5048A_ANGLE_SYN_SPEED
|
||
//磁编码器测速，数据清0
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_a = 0;
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_b = 0;
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c = 0;
|
||
|
||
//磁编码器测速，测速起始角度
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_a = g_ptz.vert_as5048a.as5048a_angle_actual;
|
||
|
||
//PID测速延时
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
|
||
|
||
//磁编码器测速，测速结束角度
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_b = g_ptz.vert_as5048a.as5048a_angle_actual;
|
||
|
||
//根据磁编码器转动的方向
|
||
//由于测量时间很短，磁编码转不了一圈，不用考虑磁编码器转动一圈的状态
|
||
//正转，云台角度增大，磁编码角度增大
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
|
||
{
|
||
if(g_ptz.vert_as5048a.as5048a_speed_angle_a <= g_ptz.vert_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_b - g_ptz.vert_as5048a.as5048a_speed_angle_a;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.vert_as5048a.as5048a_speed_angle_a) +
|
||
(g_ptz.vert_as5048a.as5048a_speed_angle_b - 0);
|
||
}
|
||
}
|
||
//反转，云台角度减小，磁编码角度减小
|
||
if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
if(g_ptz.vert_as5048a.as5048a_speed_angle_a >= g_ptz.vert_as5048a.as5048a_speed_angle_b)
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_a - g_ptz.vert_as5048a.as5048a_speed_angle_b;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c =
|
||
(360.0 - g_ptz.vert_as5048a.as5048a_speed_angle_b) +
|
||
(g_ptz.vert_as5048a.as5048a_speed_angle_a - 0);
|
||
}
|
||
}
|
||
|
||
//去除转角异常情况
|
||
if(g_ptz.vert_as5048a.as5048a_speed_angle_c < 0 ||
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c >= 50.0)
|
||
{
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c = g_ptz.vert_as5048a.as5048a_speed_angle_c_last;
|
||
}
|
||
|
||
g_ptz.vert_as5048a.as5048a_speed_angle_c_last = g_ptz.vert_as5048a.as5048a_speed_angle_c;
|
||
|
||
//计算磁编码器转速
|
||
g_ptz.vert_as5048a.as5048a_speed_actual = (g_ptz.vert_as5048a.as5048a_speed_angle_c * 1000) /
|
||
(360.0 * PTZ_VERT_PID_T) * 60.0;
|
||
//根据磁编码器计算电机转速
|
||
g_ptz.vert_motor_speed_as5048a_actual =
|
||
g_ptz.vert_as5048a.as5048a_speed_actual * PTZ_VERT_MOTOR_RATIO;
|
||
g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_as5048a_actual;
|
||
//根据磁编码器计算云台转速
|
||
g_ptz.vert_speed_as5048a_actual =
|
||
g_ptz.vert_as5048a.as5048a_speed_actual / PTZ_VERT_BIG_GEAR_RATIO;
|
||
|
||
g_ptz.vert_speed_actual = g_ptz.vert_speed_as5048a_actual;
|
||
#endif
|
||
|
||
#ifdef PTZ_VERT_PID_JY02A_SPEED
|
||
//电机驱动芯片测速，数据清0
|
||
g_ptz.vert_jy02a.jy02a_fg_num_last = g_ptz.vert_jy02a.jy02a_fg_num_actual;
|
||
g_ptz.vert_jy02a.jy02a_fg_num_actual = 0;
|
||
g_ptz.vert_motor_speed_actual = 0;
|
||
|
||
//PID测速延时
|
||
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
|
||
|
||
//根据电机驱动芯片计算电机实际转速
|
||
g_ptz.vert_motor_speed_jy02a_actual =
|
||
g_ptz.vert_jy02a.jy02a_fg_num_actual /
|
||
PTZ_VERT_FG_PWM_NUM_CYCLE *
|
||
1000.0 * 60 / PTZ_VERT_PID_T;
|
||
g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_jy02a_actual;
|
||
//根据电机驱动芯片计算云台转速
|
||
g_ptz.vert_speed_jy02a_actual = g_ptz.vert_motor_speed_jy02a_actual / PTZ_VERT_RATIO;
|
||
|
||
g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual;
|
||
#endif
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
|
||
g_ptz.vert_pid.hall_h1_count = 0;
|
||
g_ptz.vert_pid.hall_h2_count = 0;
|
||
g_ptz.vert_pid.hall_h3_count = 0;
|
||
g_ptz.vert_pid.hall_h123_count = 0;
|
||
|
||
g_ptz.vert_pid.hall_h1_count_last = 0;
|
||
g_ptz.vert_pid.hall_h1_count_time = 0;
|
||
g_ptz.vert_pid.hall_h1_count_time_s = 0;
|
||
g_ptz.vert_pid.hall_h1_count_time_e = 0;
|
||
|
||
g_ptz.vert_pid.hall_h2_count_last = 0;
|
||
g_ptz.vert_pid.hall_h2_count_time = 0;
|
||
g_ptz.vert_pid.hall_h2_count_time_s = 0;
|
||
g_ptz.vert_pid.hall_h2_count_time_e = 0;
|
||
|
||
g_ptz.vert_pid.hall_h3_count_last = 0;
|
||
g_ptz.vert_pid.hall_h3_count_time = 0;
|
||
g_ptz.vert_pid.hall_h3_count_time_s = 0;
|
||
g_ptz.vert_pid.hall_h3_count_time_e = 0;
|
||
|
||
g_ptz.vert_pid.hall_h123_count_last = 0;
|
||
g_ptz.vert_pid.hall_h123_count_time = 0;
|
||
g_ptz.vert_pid.hall_h123_count_time_s = 0;
|
||
g_ptz.vert_pid.hall_h123_count_time_e = 0;
|
||
|
||
g_ptz.vert_pid.hall_h1_start_flag = 0;
|
||
g_ptz.vert_pid.hall_h2_start_flag = 0;
|
||
g_ptz.vert_pid.hall_h3_start_flag = 0;
|
||
g_ptz.vert_pid.hall_h123_start_flag = 0;
|
||
|
||
for(time = 0; time < PTZ_VERT_PID_T_MAX; time ++)
|
||
{
|
||
if(g_ptz.vert_pid.hall_h1_count != g_ptz.vert_pid.hall_h1_count_last)
|
||
{
|
||
if(g_ptz.vert_pid.hall_h1_start_flag == 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h1_count_last = g_ptz.vert_pid.hall_h1_count;
|
||
g_ptz.vert_pid.hall_h1_count_time_s = time;
|
||
g_ptz.vert_pid.hall_h1_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h1_count_last = g_ptz.vert_pid.hall_h1_count;
|
||
g_ptz.vert_pid.hall_h1_count_time_e = time;
|
||
}
|
||
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.vert_pid.hall_h123_start_flag == 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time_s = time;
|
||
g_ptz.vert_pid.hall_h123_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time_e = time;
|
||
}
|
||
}
|
||
|
||
if(g_ptz.vert_pid.hall_h2_count != g_ptz.vert_pid.hall_h2_count_last)
|
||
{
|
||
if(g_ptz.vert_pid.hall_h2_start_flag == 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h2_count_last = g_ptz.vert_pid.hall_h2_count;
|
||
g_ptz.vert_pid.hall_h2_count_time_s = time;
|
||
g_ptz.vert_pid.hall_h2_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h2_count_last = g_ptz.vert_pid.hall_h2_count;
|
||
g_ptz.vert_pid.hall_h2_count_time_e = time;
|
||
}
|
||
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.vert_pid.hall_h123_start_flag == 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time_s = time;
|
||
g_ptz.vert_pid.hall_h123_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time_e = time;
|
||
}
|
||
}
|
||
|
||
if(g_ptz.vert_pid.hall_h3_count != g_ptz.vert_pid.hall_h3_count_last)
|
||
{
|
||
if(g_ptz.vert_pid.hall_h3_start_flag == 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h3_count_last = g_ptz.vert_pid.hall_h3_count;
|
||
g_ptz.vert_pid.hall_h3_count_time_s = time;
|
||
g_ptz.vert_pid.hall_h3_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h3_count_last = g_ptz.vert_pid.hall_h3_count;
|
||
g_ptz.vert_pid.hall_h3_count_time_e = time;
|
||
}
|
||
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.vert_pid.hall_h123_start_flag == 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time_s = time;
|
||
g_ptz.vert_pid.hall_h123_start_flag = 1;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time_e = time;
|
||
}
|
||
}
|
||
|
||
if(time >= PTZ_VERT_PID_T)//到达默认采集周期
|
||
{
|
||
if(g_ptz.vert_pid.hall_h1_count >= 2 &&
|
||
g_ptz.vert_pid.hall_h2_count >= 2 &&
|
||
g_ptz.vert_pid.hall_h3_count >= 2)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
|
||
}
|
||
|
||
//计算时间
|
||
if(g_ptz.vert_pid.hall_h1_count_time_e > g_ptz.vert_pid.hall_h1_count_time_s)
|
||
{
|
||
g_ptz.vert_pid.hall_h1_count_time =
|
||
g_ptz.vert_pid.hall_h1_count_time_e - g_ptz.vert_pid.hall_h1_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h1_count_time = time;
|
||
}
|
||
|
||
if(g_ptz.vert_pid.hall_h2_count_time_e > g_ptz.vert_pid.hall_h2_count_time_s)
|
||
{
|
||
g_ptz.vert_pid.hall_h2_count_time =
|
||
g_ptz.vert_pid.hall_h2_count_time_e - g_ptz.vert_pid.hall_h2_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h2_count_time = time;
|
||
}
|
||
|
||
if(g_ptz.vert_pid.hall_h3_count_time_e > g_ptz.vert_pid.hall_h3_count_time_s)
|
||
{
|
||
g_ptz.vert_pid.hall_h3_count_time =
|
||
g_ptz.vert_pid.hall_h3_count_time_e - g_ptz.vert_pid.hall_h3_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h3_count_time = time;
|
||
}
|
||
|
||
//计算接收所有霍尔脉冲的时间
|
||
if(g_ptz.vert_pid.hall_h123_count_time_e > g_ptz.vert_pid.hall_h123_count_time_s)
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time =
|
||
g_ptz.vert_pid.hall_h123_count_time_e - g_ptz.vert_pid.hall_h123_count_time_s;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h123_count_time = time;
|
||
}
|
||
|
||
//根据三个霍尔的反馈情况分别计算转速
|
||
i = 0;
|
||
if(g_ptz.vert_pid.hall_h1_count >= 2 && g_ptz.vert_pid.hall_h1_count_time > 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h1_motor_speed =
|
||
60000.0 * (float)(g_ptz.vert_pid.hall_h1_count - 1) /
|
||
(float)g_ptz.vert_pid.hall_h1_count_time /
|
||
PTZ_VERT_MOTOR_POLE_PAIRS;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h1_motor_speed = 0;
|
||
}
|
||
|
||
|
||
if(g_ptz.vert_pid.hall_h2_count >= 2 && g_ptz.vert_pid.hall_h2_count_time > 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h2_motor_speed =
|
||
60000.0 * (float)(g_ptz.vert_pid.hall_h2_count - 1) /
|
||
(float)g_ptz.vert_pid.hall_h2_count_time /
|
||
PTZ_VERT_MOTOR_POLE_PAIRS;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h2_motor_speed = 0;
|
||
}
|
||
|
||
|
||
if(g_ptz.vert_pid.hall_h3_count >= 2 && g_ptz.vert_pid.hall_h3_count_time > 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h3_motor_speed =
|
||
60000.0 * (float)(g_ptz.vert_pid.hall_h3_count - 1) /
|
||
(float)g_ptz.vert_pid.hall_h3_count_time /
|
||
PTZ_VERT_MOTOR_POLE_PAIRS;
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h3_motor_speed = 0;
|
||
}
|
||
|
||
g_ptz.vert_pid.hall_h123_count = g_ptz.vert_pid.hall_h1_count +
|
||
g_ptz.vert_pid.hall_h2_count +
|
||
g_ptz.vert_pid.hall_h3_count;
|
||
//根据霍尔总脉冲计算电机转速
|
||
if(g_ptz.vert_pid.hall_h123_count >= 2 && g_ptz.vert_pid.hall_h123_count_time > 0)
|
||
{
|
||
g_ptz.vert_pid.hall_h123_motor_speed =
|
||
60000.0 * (float)(g_ptz.vert_pid.hall_h123_count - 1) /
|
||
(float)g_ptz.vert_pid.hall_h123_count_time /
|
||
PTZ_VERT_ONE_CYCLE_HALL_NUM;
|
||
|
||
i++;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.hall_h123_motor_speed = 0;
|
||
}
|
||
|
||
|
||
//计算电机平均转速
|
||
if(i > 0)
|
||
{
|
||
g_ptz.vert_motor_speed_hall_actual = (g_ptz.vert_pid.hall_h1_motor_speed +
|
||
g_ptz.vert_pid.hall_h2_motor_speed +
|
||
g_ptz.vert_pid.hall_h3_motor_speed +
|
||
g_ptz.vert_pid.hall_h123_motor_speed) / i;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_motor_speed_hall_actual = 0;
|
||
}
|
||
|
||
|
||
//判断电机转动是否抖动
|
||
if(g_ptz.vert_pid.hall_h1_count == 0 ||
|
||
g_ptz.vert_pid.hall_h2_count == 0 ||
|
||
g_ptz.vert_pid.hall_h3_count == 0)
|
||
{//电机在抖动，速度直接置0
|
||
g_ptz.vert_motor_speed_hall_actual = 0;
|
||
}
|
||
|
||
g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_hall_actual;
|
||
|
||
//根据电机转速计算云台转速
|
||
g_ptz.vert_speed_hall_actual = g_ptz.vert_motor_speed_hall_actual / PTZ_VERT_RATIO;
|
||
g_ptz.vert_speed_actual = g_ptz.vert_speed_hall_actual;
|
||
|
||
#endif
|
||
|
||
}
|
||
|
||
//PID调速
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
|
||
g_ptz.vert_pid.mode == PID_ADJUST_SPEED)
|
||
{
|
||
|
||
#ifdef PTZ_CONTROL_SPEED_L6235D
|
||
//判断是否需要启动方向调速
|
||
if(g_ptz.vert_pid.PidUT_float <= 0 &&
|
||
g_ptz.vert_speed_actual > g_ptz.vert_speed_set)
|
||
{
|
||
adc7311_vert_vr(0);
|
||
g_ptz.vert_pid.director_speed_state = 0;
|
||
g_ptz.vert_pid.mode = DIRECTOR_ADJUST_SPEED;
|
||
}
|
||
#endif
|
||
|
||
|
||
|
||
/*由于系统实时调节能力不足将调速分为两个部分：
|
||
1.启动时首先进入直线比例快速提速；
|
||
2.速度达到设定速度的70％后转换为PID调速。*/
|
||
|
||
|
||
if(g_ptz.vert_speed_actual < g_ptz.vert_speed_set * PTZ_VERT_PRO_ADD_RANGE_LESS)
|
||
{
|
||
// if(time > PTZ_VERT_PID_ADD)
|
||
// {
|
||
// time = PTZ_VERT_PID_ADD;
|
||
// }
|
||
// g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float + time/*PTZ_VERT_PID_ADD*/;
|
||
// //存储误差，用于PID计算
|
||
// g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
|
||
// g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
|
||
|
||
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float * PTZ_VERT_PRO_ADD_MUL_LESS;
|
||
if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_PID_ADD)
|
||
{
|
||
g_ptz.vert_pid.PidUT_float = PTZ_VERT_PID_ADD;
|
||
}
|
||
//存储误差，用于PID计算
|
||
g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
|
||
g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
|
||
}
|
||
// else if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set + PTZ_VERT_PRO_ADD_RANGE_MORE)
|
||
// {
|
||
// g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float * PTZ_VERT_PRO_ADD_MUL_MORE;
|
||
// //存储误差，用于PID计算
|
||
// g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
|
||
// g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
|
||
// }
|
||
else
|
||
{
|
||
//计算PID控制器输出值
|
||
g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float;
|
||
|
||
//控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
|
||
//即防止PID增量过大
|
||
if(fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT)//限定PID输出限定
|
||
{
|
||
if(g_ptz.vert_pid.PidUT_float > g_ptz.vert_pid.LastUT_float)
|
||
{
|
||
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float + PTZ_VERT_PID_OUTPUT_LIMIT;
|
||
}
|
||
else
|
||
{
|
||
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float - PTZ_VERT_PID_OUTPUT_LIMIT;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_VR_MIN)//限制输入模拟电压最小值
|
||
{
|
||
g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MIN;
|
||
}
|
||
if(g_ptz.vert_pid.PidUT_float > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值
|
||
{
|
||
g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MAX;
|
||
}
|
||
|
||
g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5);
|
||
|
||
//限制PID的输出值在某个指定的范围
|
||
if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//限制输入模拟电压最小值
|
||
{
|
||
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN;
|
||
}
|
||
if(g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值
|
||
{
|
||
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX;
|
||
}
|
||
|
||
|
||
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
|
||
vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint);
|
||
//将当前PID输出值保存
|
||
g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float;
|
||
}
|
||
|
||
|
||
|
||
#ifdef PTZ_CONTROL_SPEED_L6235D
|
||
//方向pwm波反向力矩调速
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
|
||
g_ptz.vert_pid.mode == DIRECTOR_ADJUST_SPEED)
|
||
{
|
||
/******/
|
||
//判断是否需要恢复PID调速
|
||
if(g_ptz.vert_pid.director_speed_start_t <= 0 &&
|
||
g_ptz.vert_speed_actual < (g_ptz.vert_speed_set / 4.0))
|
||
{
|
||
adc7311_vert_vr(0);
|
||
g_ptz.vert_pid.director_speed_state = 0;
|
||
// g_ptz.vert_pid.SumError = 0;//累计误差
|
||
// g_ptz.vert_pid.LastError = 0;//上一次输入偏差
|
||
// g_ptz.vert_pid.PrevError = 0;//上上次输入偏差
|
||
// g_ptz.vert_pid.LastUT_float = 0;//上次PID输出值u(t)
|
||
// g_ptz.vert_pid.PidUT_float = 0;//PID当前输出值
|
||
l6235d_vert_set_direction(g_ptz.vert_direction_set);
|
||
g_ptz.vert_pid.mode = PID_ADJUST_SPEED;
|
||
continue;
|
||
}
|
||
/*****/
|
||
switch(g_ptz.vert_pid.director_speed_state)
|
||
{
|
||
case 0://方向调速初始化
|
||
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
|
||
{
|
||
g_ptz.vert_pid.director_speed_direction = PTZ_VERT_DIR_UP;
|
||
}
|
||
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
|
||
{
|
||
g_ptz.vert_pid.director_speed_direction = PTZ_VERT_DIR_DOWN;
|
||
}
|
||
g_ptz.vert_pid.director_speed_t = PTZ_DIRECTOR_SPEED_T;
|
||
g_ptz.vert_pid.director_speed_vref = 0;
|
||
adc7311_vert_vr(g_ptz.vert_pid.director_speed_vref);
|
||
g_ptz.vert_pid.director_speed_start_t = 0;
|
||
g_ptz.vert_pid.director_speed_stop_t = PTZ_DIRECTOR_SPEED_T - g_ptz.vert_pid.director_speed_start_t;
|
||
g_ptz.vert_pid.director_speed_state ++;
|
||
break;
|
||
|
||
case 1://调节方向占空比
|
||
if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set &&
|
||
g_ptz.vert_pid.director_speed_start_t < g_ptz.vert_pid.director_speed_t)
|
||
{
|
||
g_ptz.vert_pid.director_speed_start_t ++;
|
||
}
|
||
if(g_ptz.vert_speed_actual < g_ptz.vert_speed_set &&
|
||
g_ptz.vert_pid.director_speed_start_t > 0)
|
||
{
|
||
g_ptz.vert_pid.director_speed_start_t --;
|
||
}
|
||
g_ptz.vert_pid.director_speed_stop_t =
|
||
g_ptz.vert_pid.director_speed_t - g_ptz.vert_pid.director_speed_start_t;
|
||
break;
|
||
}
|
||
}
|
||
#endif
|
||
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
||
{
|
||
g_ptz.vert_pid.roll_start = 0;
|
||
g_ptz.vert_speed_actual = 0;
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
}
|
||
|
||
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
|
||
//防止数据超限溢出
|
||
if(g_ptz.vert_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.vert_pid.hall_h1_count = 0;
|
||
}
|
||
if(g_ptz.vert_pid.hall_h2_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.vert_pid.hall_h2_count = 0;
|
||
}
|
||
if(g_ptz.vert_pid.hall_h3_count >= UNSIGNED_INT_MAX_VALUE)
|
||
{
|
||
g_ptz.vert_pid.hall_h3_count = 0;
|
||
}
|
||
#endif
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
//方向调速任务,方向PWM波产生任务
|
||
static void ptz_vert_director_speed_pwm_task()
|
||
{
|
||
while(1)
|
||
{//方向pwm波
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
|
||
g_ptz.vert_pid.mode == DIRECTOR_ADJUST_SPEED)
|
||
{
|
||
switch(g_ptz.vert_pid.director_speed_state)
|
||
{
|
||
case 1:
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
|
||
g_ptz.vert_pid.director_speed_start_t > 0)
|
||
{
|
||
l6235d_vert_set_direction(g_ptz.vert_pid.director_speed_direction);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_pid.director_speed_start_t);
|
||
}
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
|
||
g_ptz.vert_pid.director_speed_stop_t > 0)
|
||
{
|
||
l6235d_vert_set_direction(g_ptz.vert_direction_actual);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_pid.director_speed_stop_t);
|
||
}
|
||
break;
|
||
|
||
default:
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
}
|
||
}
|
||
}
|
||
|
||
static OS_STK task_vert_pid_stk[TASK_VERT_PID_STK_SIZE];
|
||
static void creat_task_vert_pid(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_pid_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_vert_pid_stk[TASK_VERT_PID_STK_SIZE - 1],
|
||
(INT8U ) TASK_VERT_PID_PRIO,
|
||
(INT16U ) TASK_VERT_PID_PRIO,
|
||
(OS_STK *)&task_vert_pid_stk[0],
|
||
(INT32U ) TASK_VERT_PID_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_pid_task", &name_err);
|
||
#endif
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
static OS_STK task_vert_director_speed_pwm_stk[TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE];
|
||
static void creat_task_director_speed_pwm(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_director_speed_pwm_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_vert_director_speed_pwm_stk[TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE - 1],
|
||
(INT8U ) TASK_VERT_DIRECTOR_SPEED_PWM_PRIO,
|
||
(INT16U ) TASK_VERT_DIRECTOR_SPEED_PWM_PRIO,
|
||
(OS_STK *)&task_vert_director_speed_pwm_stk[0],
|
||
(INT32U ) TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE,
|
||
(void *) 0,
|
||
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
|
||
#if (OS_TASK_NAME_EN > 0)
|
||
OSTaskNameSet(TASK_VERT_DIRECTOR_SPEED_PWM_PRIO, "ptz_vert_director_speed_pwm_task", &name_err);
|
||
#endif
|
||
|
||
|
||
}
|
||
/******************************************************************************/
|
||
|
||
|
||
void init_speed_module(void)
|
||
{
|
||
ptz_pid_init();
|
||
creat_task_hori_pid();
|
||
creat_task_vert_pid();
|
||
creat_task_director_speed_pwm();
|
||
}
|
||
#endif
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|