150 lines
4.1 KiB
C
150 lines
4.1 KiB
C
#include "speed_to_servoMotor.h"
|
|
#include "ptz_header_file.h"
|
|
#include "servoMotor.h"
|
|
|
|
#ifdef PTZ_SERVO_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;
|
|
}
|
|
}
|
|
|
|
|
|
static void ptz_hori_step_speed_task()
|
|
{
|
|
while(1)
|
|
{
|
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
|
{
|
|
}
|
|
|
|
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
|
|
{
|
|
}
|
|
OSTimeDlyHMSM(0u, 0u, 0u, 10);
|
|
}
|
|
}
|
|
|
|
|
|
static void ptz_vert_step_speed_task()
|
|
{
|
|
while(1)
|
|
{
|
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
|
{
|
|
}
|
|
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
|
|
{
|
|
}
|
|
OSTimeDlyHMSM(0u, 0u, 0u, 10);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static OS_STK task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE];
|
|
static void creat_task_hori_step_speed_task(void)
|
|
{
|
|
CPU_INT08U task_err;
|
|
CPU_INT08U name_err;
|
|
|
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_step_speed_task,
|
|
(void *) 0,
|
|
(OS_STK *)&task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE - 1],
|
|
(INT8U ) TASK_HORI_PID_PRIO,
|
|
(INT16U ) TASK_HORI_PID_PRIO,
|
|
(OS_STK *)&task_hori_step_speed_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_step_speed_task", &name_err);
|
|
#endif
|
|
|
|
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
|
pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_step_speed_task success...\n\r");
|
|
} else {
|
|
pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_step_speed_task failed...\n\r");
|
|
}
|
|
}
|
|
|
|
|
|
|
|
static OS_STK task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE];
|
|
static void creat_task_vert_step_speed_task(void)
|
|
{
|
|
CPU_INT08U task_err;
|
|
CPU_INT08U name_err;
|
|
|
|
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_step_speed_task,
|
|
(void *) 0,
|
|
(OS_STK *)&task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE - 1],
|
|
(INT8U ) TASK_VERT_PID_PRIO,
|
|
(INT16U ) TASK_VERT_PID_PRIO,
|
|
(OS_STK *)&task_vert_step_speed_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_step_speed_task", &name_err);
|
|
#endif
|
|
|
|
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
|
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_step_speed_task success...\n\r");
|
|
} else {
|
|
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_step_speed_task failed...\n\r");
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
void init_speed_module(void)
|
|
{
|
|
servoMotorInit();
|
|
creat_task_hori_step_speed_task();
|
|
creat_task_vert_step_speed_task();
|
|
}
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|