servoMotor/APP/Device/Device_speed/servoMotor_recv.c

351 lines
12 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#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