pengsx
/
MW22-02A
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
MW22-02A/APP/Device/Device_speed/speed_to_bldc.c

1448 lines
48 KiB
C
Raw Permalink Normal View History

汇源原始工程
2025-05-21 01:55:40 +00:00
#include "ptz_header_file.h"
#include <math.h>
#include "app_cfg.h"
#include "gd32f4xx.h"
#include "device_dac_out.h"
#ifdef PTZ_BLDC_MOTOR
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ʵ<CCA8><CAB5>ת<EFBFBD><D7AA>
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;//ˮƽ<CBAE><C6BD><EFBFBD><EFBFBD>
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;//<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>
Speed2[4] = (unsigned char)(VSpeed >> 8);
Speed2[5] = (unsigned char)(VSpeed & 0x00ff);
Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2));
switch(speed)
{
case 1://ֻ<><D6BB>ѯˮƽת<C6BD><D7AA>
ptz_send_data(dev,Speed1,sizeof(Speed1));
break;
case 2://ֻ<><D6BB>ѯ<EFBFBD><D1AF>ֱת<D6B1><D7AA>
ptz_send_data(dev,Speed2,sizeof(Speed2));
break;
default://ˮƽת<C6BD>ٴ<EFBFBD>ֱת<D6B1>ٶ<EFBFBD><D9B6><EFBFBD>ѯ
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;//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_pid.LastError = 0;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.hori_pid.PrevError = 0;//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.hori_pid.LastUT_float = 0;//<2F>ϴ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>ֵu(t)
g_ptz.hori_pid.PidUT_float = 0;//PID<49><44>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
g_ptz.hori_pid.KP = PTZ_HORI_PID_HORI_KP;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_pid.TI = PTZ_HORI_PID_HORI_TI;//<2F><><EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD>
g_ptz.hori_pid.TD = PTZ_HORI_PID_HORI_TD;//΢<>ֳ<EFBFBD><D6B3><EFBFBD>
g_ptz.hori_pid.T = PTZ_HORI_PID_T / 1000.0;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD>򻯲<EFBFBD><F2BBAFB2><EFBFBD>A<EFBFBD><41>B<EFBFBD><42>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;//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_pid.LastError = 0;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.vert_pid.PrevError = 0;//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.vert_pid.LastUT_float = 0;//<2F>ϴ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>ֵu(t)
g_ptz.vert_pid.PidUT_float = 0;//PID<49><44>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
g_ptz.vert_pid.KP = PTZ_VERT_PID_VERT_KP;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_pid.TI = PTZ_VERT_PID_VERT_TI;//<2F><><EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD>
g_ptz.vert_pid.TD = PTZ_VERT_PID_VERT_TD;//΢<>ֳ<EFBFBD><D6B3><EFBFBD>
g_ptz.vert_pid.T = PTZ_VERT_PID_T / 1000.0;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><><EFBFBD><EFBFBD><EFBFBD>򻯲<EFBFBD><F2BBAFB2><EFBFBD>A<EFBFBD><41>B<EFBFBD><42>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;//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_pid.LastError = 0;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.hori_pid.PrevError = 0;//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.hori_pid.LastUT_float = 0;//<2F>ϴ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>ֵu(t)
g_ptz.hori_pid.PidUT_float = 0;//PID<49><44>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
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;//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>PID_INPUT_LIMIT
/*<2A><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
// 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;
// }
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD>´μ<C2B4><CEBC><EFBFBD>
g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
g_ptz.hori_pid.LastError = H_IError;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
return H_IIncPid;
}
//<2F>µ<EFBFBD><C2B5>ٷ<EFBFBD>ʽ
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
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD>Ƕ<EFBFBD>
g_ptz.hori_as5048a.as5048a_speed_angle_a = g_ptz.hori_as5048a.as5048a_angle_actual;
//PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
g_ptz.hori_as5048a.as5048a_speed_angle_b = g_ptz.hori_as5048a.as5048a_angle_actual;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>̣ܶ<DCB6><CCA3>ű<EFBFBD><C5B1><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>Ǵű<C7B4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>һȦ<D2BB><C8A6>״̬
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
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);
}
}
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>󣬴ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
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);
}
}
//ȥ<><C8A5>ת<EFBFBD><D7AA><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD>Ƕ<EFBFBD>
g_ptz.hori_as5048a.as5048a_speed_angle_a = g_ptz.hori_as5048a.as5048a_angle_actual;
//PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
g_ptz.hori_as5048a.as5048a_speed_angle_b = g_ptz.hori_as5048a.as5048a_angle_actual;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>̣ܶ<DCB6><CCA3>ű<EFBFBD><C5B1><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>Ǵű<C7B4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>һȦ<D2BB><C8A6>״̬
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>󣬴ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
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);
}
}
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
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);
}
}
//ȥ<><C8A5>ת<EFBFBD><D7AA><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE><C6AC><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_HORI_PID_T);
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE><C6AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE><C6AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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)//<2F><><EFBFBD><EFBFBD>Ĭ<EFBFBD>ϲɼ<CFB2><C9BC><EFBFBD><EFBFBD><EFBFBD>
{
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);
}
//<2F><><EFBFBD>㵥<EFBFBD><E3B5A5>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݻ<EFBFBD><DDBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ת<EFBFBD><D7AA>
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;
}
//<2F>жϵ<D0B6><CFB5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ񶶶<C7B7>
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)
{//<2F><><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>0
g_ptz.hori_motor_speed_hall_actual = 0;
}
g_ptz.hori_motor_speed_actual = g_ptz.hori_motor_speed_hall_actual;
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD>ת<EFBFBD>ټ<EFBFBD><D9BC><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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
/*<2A><><EFBFBD><EFBFBD>ϵͳʵʱ<CAB5><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㽫<EFBFBD><E3BDAB><EFBFBD>ٷ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֣<EFBFBD>
1.<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>Ƚ<EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߱<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD>
2.<EFBFBD>ٶȴﵽ<EFBFBD>趨<EFBFBD>ٶȵ<EFBFBD>60<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>ΪPID<EFBFBD><EFBFBD><EFBFBD>١<EFBFBD>*/
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*/;
// //<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
// 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;
}
//<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
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;
/**<2A><><EFBFBD>dz<EFBFBD><C7B3><EFBFBD>ʱ<EFBFBD><CAB1>ֱ<EFBFBD>߽<EFBFBD><DFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>򳬵<EFBFBD><F2B3ACB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٻ<EFBFBD><D9BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1>м<EFBFBD><D0BC><EFBFBD>**/
}
else
{
//<2F><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float;
//<2F><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3><EFBFBD><EFBFBD>Χ<EFBFBD><CEA7>
//<2F><><EFBFBD><EFBFBD>ֹPID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(fabs(g_ptz.hori_pid.PidUT_float - g_ptz.hori_pid.LastUT_float) > PTZ_HORI_PID_OUTPUT_LIMIT)//<2F>޶<EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD>޶<EFBFBD>
{
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)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>Сֵ
{
g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MIN;
}
if(g_ptz.hori_pid.PidUT_float > PTZ_HORI_VR_MAX)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>ֵ
{
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);
//<2F><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ij<EFBFBD><C4B3>ָ<EFBFBD><D6B8><EFBFBD>ķ<EFBFBD>Χ
if(g_ptz.hori_pid.PidUT_uint <= PTZ_HORI_VR_MIN)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>Сֵ
{
g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MIN;
}
if(g_ptz.hori_pid.PidUT_uint > PTZ_HORI_VR_MAX)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>ֵ
{
g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MAX;
}
//<2F><>PID<49><44><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ת<EFBFBD><D7AA>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD>뵽ģ<EBB5BD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>оƬ
hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint);
//<2F><><EFBFBD><EFBFBD>ǰPID<49><44><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_pid.LastError = 0;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.vert_pid.PrevError = 0;//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
g_ptz.vert_pid.LastUT_float = 0;//<2F>ϴ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>ֵu(t)
g_ptz.vert_pid.PidUT_float = 0;//PID<49><44>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
g_ptz.vert_pid.PidUT_uint = 0;
g_ptz.vert_pid.mode = PID_ADJUST_SPEED;//<2F><><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>PID<49><44><EFBFBD><EFBFBD>
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;//<2F><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>PID_INPUT_LIMIT
/*<2A><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
// 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;
// }
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
//<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD>´μ<C2B4><CEBC><EFBFBD>
g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
g_ptz.vert_pid.LastError = IError;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
return IIncPid;
}
//<2F>µ<EFBFBD><C2B5>ٷ<EFBFBD>ʽ
static void ptz_vert_pid_task()
{
unsigned int time = 0;
char i = 0;
while(1)
{
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
#ifdef PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD>Ƕ<EFBFBD>
g_ptz.vert_as5048a.as5048a_speed_angle_a = g_ptz.vert_as5048a.as5048a_angle_actual;
//PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
g_ptz.vert_as5048a.as5048a_speed_angle_b = g_ptz.vert_as5048a.as5048a_angle_actual;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>̣ܶ<DCB6><CCA3>ű<EFBFBD><C5B1><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>Ǵű<C7B4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>һȦ<D2BB><C8A6>״̬
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
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);
}
}
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>󣬴ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
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);
}
}
//ȥ<><C8A5>ת<EFBFBD><D7AA><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
g_ptz.vert_as5048a.as5048a_speed_actual = (g_ptz.vert_as5048a.as5048a_speed_angle_c * 1000) /
(360.0 * PTZ_VERT_PID_T) * 60.0;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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;
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD>Ƕ<EFBFBD>
g_ptz.vert_as5048a.as5048a_speed_angle_a = g_ptz.vert_as5048a.as5048a_angle_actual;
//PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
//<2F>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD><EFBFBD>Ƕ<EFBFBD>
g_ptz.vert_as5048a.as5048a_speed_angle_b = g_ptz.vert_as5048a.as5048a_angle_actual;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>
//<2F><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>̣ܶ<DCB6><CCA3>ű<EFBFBD><C5B1><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>һȦ<D2BB><C8A6><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF>Ǵű<C7B4><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>һȦ<D2BB><C8A6>״̬
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD>󣬴ű<F3A3ACB4><C5B1><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
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);
}
}
//<2F><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>̨<EFBFBD>Ƕȼ<C7B6>С<EFBFBD><D0A1><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD>Ƕȼ<C7B6>С
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);
}
}
//ȥ<><C8A5>ת<EFBFBD><D7AA><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD>
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;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
g_ptz.vert_as5048a.as5048a_speed_actual = (g_ptz.vert_as5048a.as5048a_speed_angle_c * 1000) /
(360.0 * PTZ_VERT_PID_T) * 60.0;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݴű<DDB4><C5B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE><C6AC><EFBFBD>٣<EFBFBD><D9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>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<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
OSTimeDlyHMSM(0u, 0u, 0u, PTZ_VERT_PID_T);
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE><C6AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>оƬ<D0BE><C6AC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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)//<2F><><EFBFBD><EFBFBD>Ĭ<EFBFBD>ϲɼ<CFB2><C9BC><EFBFBD><EFBFBD><EFBFBD>
{
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);
}
//<2F><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
//<2F><><EFBFBD>ݻ<EFBFBD><DDBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
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;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD>ת<EFBFBD><D7AA>
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;
}
//<2F>жϵ<D0B6><CFB5><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ񶶶<C7B7>
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)
{//<2F><><EFBFBD><EFBFBD><EFBFBD>ڶ<EFBFBD><DAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>0
g_ptz.vert_motor_speed_hall_actual = 0;
}
g_ptz.vert_motor_speed_actual = g_ptz.vert_motor_speed_hall_actual;
//<2F><><EFBFBD>ݵ<EFBFBD><DDB5><EFBFBD>ת<EFBFBD>ټ<EFBFBD><D9BC><EFBFBD><EFBFBD><EFBFBD>̨ת<CCA8><D7AA>
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<49><44><EFBFBD><EFBFBD>
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
g_ptz.vert_pid.mode == PID_ADJUST_SPEED)
{
#ifdef PTZ_CONTROL_SPEED_L6235D
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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
/*<2A><><EFBFBD><EFBFBD>ϵͳʵʱ<CAB5><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㽫<EFBFBD><E3BDAB><EFBFBD>ٷ<EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֣<EFBFBD>
1.<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>Ƚ<EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߱<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>٣<EFBFBD>
2.<EFBFBD>ٶȴﵽ<EFBFBD>趨<EFBFBD>ٶȵ<EFBFBD>70<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>ΪPID<EFBFBD><EFBFBD><EFBFBD>١<EFBFBD>*/
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*/;
// //<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
// 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;
}
//<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
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;
// //<2F>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD>
// 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
{
//<2F><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float;
//<2F><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD>IJ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3><EFBFBD><EFBFBD>Χ<EFBFBD><CEA7>
//<2F><><EFBFBD><EFBFBD>ֹPID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if(fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT)//<2F>޶<EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD>޶<EFBFBD>
{
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)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>Сֵ
{
g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MIN;
}
if(g_ptz.vert_pid.PidUT_float > PTZ_VERT_VR_MAX)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>ֵ
{
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);
//<2F><><EFBFBD><EFBFBD>PID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ij<EFBFBD><C4B3>ָ<EFBFBD><D6B8><EFBFBD>ķ<EFBFBD>Χ
if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9>Сֵ
{
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN;
}
if(g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>ֵ
{
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX;
}
//<2F><>PID<49><44><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>ת<EFBFBD><D7AA>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD><EFBFBD>뵽ģ<EBB5BD><C4A3><EFBFBD><EFBFBD>ѹ<EFBFBD><D1B9><EFBFBD><EFBFBD>оƬ
vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint);
//<2F><><EFBFBD><EFBFBD>ǰPID<49><44><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float;
}
#ifdef PTZ_CONTROL_SPEED_L6235D
//<2F><><EFBFBD><EFBFBD>pwm<77><6D><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>
if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
g_ptz.vert_pid.mode == DIRECTOR_ADJUST_SPEED)
{
/******/
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫ<EFBFBD>ָ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>
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;//<2F>ۼ<EFBFBD><DBBC><EFBFBD><EFBFBD><EFBFBD>
// g_ptz.vert_pid.LastError = 0;//<2F><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
// g_ptz.vert_pid.PrevError = 0;//<2F><><EFBFBD>ϴ<EFBFBD><CFB4><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
// g_ptz.vert_pid.LastUT_float = 0;//<2F>ϴ<EFBFBD>PID<49><44><EFBFBD><EFBFBD>ֵu(t)
// g_ptz.vert_pid.PidUT_float = 0;//PID<49><44>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
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://<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٳ<EFBFBD>ʼ<EFBFBD><CABC>
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://<2F><><EFBFBD>ڷ<EFBFBD><DAB7><EFBFBD>ռ<EFBFBD>ձ<EFBFBD>
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 //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
//<2F><>ֹ<EFBFBD><D6B9><EFBFBD>ݳ<EFBFBD><DDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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
}
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static void ptz_vert_director_speed_pwm_task()
{
while(1)
{//<2F><><EFBFBD><EFBFBD>pwm<77><6D>
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