351 lines
12 KiB
C
351 lines
12 KiB
C
#include "speed_to_servoMotor.h"
|
||
#include "ptz_header_file.h"
|
||
#include "servoMotor.h"
|
||
|
||
|
||
#ifdef PTZ_SERVO_MOTOR
|
||
|
||
//电机数据解析任务互斥量
|
||
BSP_OS_SEM g_horiMotorMutex;
|
||
BSP_OS_SEM g_vertMotorMutex;
|
||
|
||
/**
|
||
* @brief 写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
|
||
* @param motorNo：写垂直电机还是水平电机
|
||
* @return false:错误，读出的内容与写入的内容不一致
|
||
*/
|
||
static bool MotorReplyForWrite(uint8_t motorNo)
|
||
{
|
||
static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
|
||
if (motorNo == horiMotorType )
|
||
{
|
||
if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
|
||
{
|
||
return false;
|
||
}
|
||
servoLinkListMemPut(horiMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
|
||
{
|
||
return false;
|
||
}
|
||
servoLinkListMemPut(horiMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
// servoLinkListMemPut(horiMotorType);
|
||
}
|
||
else if ( motorNo == vertMotorType )
|
||
{
|
||
if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
|
||
{
|
||
return false;
|
||
}
|
||
servoLinkListMemPut(vertMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
|
||
{
|
||
return false;
|
||
}
|
||
servoLinkListMemPut(vertMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
// servoLinkListMemPut(vertMotorType);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/**
|
||
* @brief 读单个寄存器成功后，电机会返回一帧7字节数据
|
||
* @param motorNo：写垂直电机还是水平电机
|
||
* @param speed：获取读取命令后，电机返回的数据内容
|
||
* @return false:读取返回错误
|
||
*/
|
||
|
||
static bool MotorReplyForRead(uint8_t motorNo)
|
||
{
|
||
int16_t speed = 0;
|
||
static uint8_t motorReplybuff[READ_ONE_REG_FRAME_NUM];
|
||
if (motorNo == horiMotorType )
|
||
{
|
||
if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
|
||
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
|
||
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
|
||
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
|
||
if (speed < 0)
|
||
{
|
||
speed = -speed;
|
||
}
|
||
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
|
||
servoLinkListMemPut(horiMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
|
||
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
|
||
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
|
||
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
|
||
if (speed < 0)
|
||
{
|
||
speed = -speed;
|
||
}
|
||
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
|
||
servoLinkListMemPut(horiMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
// servoLinkListMemPut(horiMotorType);
|
||
}
|
||
else if ( motorNo == vertMotorType )
|
||
{
|
||
if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
|
||
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
|
||
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
|
||
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
|
||
if (speed < 0)
|
||
{
|
||
speed = -speed;
|
||
}
|
||
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
|
||
servoLinkListMemPut(vertMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
|
||
{
|
||
|
||
if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
|
||
{
|
||
/*提取数据失败*/
|
||
if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
|
||
{
|
||
return false;
|
||
}
|
||
if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
|
||
if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
|
||
if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
|
||
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
|
||
if (speed < 0)
|
||
{
|
||
speed = -speed;
|
||
}
|
||
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
|
||
servoLinkListMemPut(vertMotorType);
|
||
}
|
||
else
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
// servoLinkListMemPut(vertMotorType);
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
/*!
|
||
\brief 水平电机接收任务
|
||
\param[in] none
|
||
\param[out] none
|
||
\retval none
|
||
*/
|
||
static void ptz_recv_hori_servo_task()
|
||
{
|
||
CPU_INT08U err;
|
||
while(1) {
|
||
OSSemPend(g_horiMotorMutex, 0, &err);
|
||
// stopTimeOut(H_MOTOR);
|
||
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
|
||
{
|
||
// servoLinkListMemPut(horiMotorType);
|
||
// H_MOTOR_STOP;
|
||
// stopTimeOut(H_MOTOR);
|
||
continue;
|
||
}
|
||
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
stopTimeOut(H_MOTOR);
|
||
//释放信号量，通知能发送一次
|
||
OSSemPost(g_horiSpeedMutex);
|
||
}
|
||
}
|
||
|
||
/*!
|
||
\brief 俯仰电机接收任务
|
||
\param[in] none
|
||
\param[out] none
|
||
\retval none
|
||
*/
|
||
static void ptz_recv_vert_servo_task()
|
||
{
|
||
CPU_INT08U err;
|
||
while(1) {
|
||
OSSemPend(g_vertMotorMutex, 0, &err);
|
||
stopTimeOut(V_MOTOR);
|
||
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false)
|
||
{
|
||
// V_MOTOR_STOP;
|
||
continue;
|
||
}
|
||
|
||
//释放信号量，通知能发送一次
|
||
OSSemPost(g_vertSpeedMutex);
|
||
}
|
||
}
|
||
|
||
static OS_STK task_recv_hori_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE];
|
||
static OS_STK task_recv_vert_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE];
|
||
static void creat_task_servo_recv_task(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_recv_hori_servo_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_recv_hori_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE - 1],
|
||
(INT8U ) TASK_RECV_HORI_SERVO_PRIO,
|
||
(INT16U ) TASK_RECV_HORI_SERVO_PRIO,
|
||
(OS_STK *)&task_recv_hori_servo_stk[0],
|
||
(INT32U ) TASK_RECV_HORI_SERVO_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_recv_hori_servo_task", &name_err);
|
||
#endif
|
||
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_recv_hori_servo_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_recv_hori_servo_task failed...\n\r");
|
||
}
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_recv_vert_servo_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_recv_vert_servo_stk[TASK_RECV_HORI_SERVO_STK_SIZE - 1],
|
||
(INT8U ) TASK_RECV_VERT_SERVO_PRIO,
|
||
(INT16U ) TASK_RECV_VERT_SERVO_PRIO,
|
||
(OS_STK *)&task_recv_vert_servo_stk[0],
|
||
(INT32U ) TASK_RECV_HORI_SERVO_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_recv_vert_servo_task", &name_err);
|
||
#endif
|
||
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_recv_vert_servo_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_recv_vert_servo_task failed...\n\r");
|
||
}
|
||
}
|
||
|
||
|
||
void Init_ServoMotorRecv(void)
|
||
{
|
||
CPU_INT08U err;
|
||
g_horiMotorMutex = OSSemCreate(1);
|
||
g_vertMotorMutex = OSSemCreate(1);
|
||
OSSemPend(g_horiMotorMutex, 1, &err);
|
||
OSSemPend(g_vertMotorMutex, 1, &err);
|
||
|
||
creat_task_servo_recv_task();
|
||
}
|
||
|
||
#endif
|