pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
servoMotor/APP/Device/Device_speed/servoMotor_recv.c

348 lines
12 KiB
C
Raw Normal View History

完成发送接收框架
2025-10-14 02:48:50 +00:00
#include "speed_to_servoMotor.h"
#include "ptz_header_file.h"
#include "servoMotor.h"
伺服电机驱动修改，取消发送缓冲区（数据缓冲和DMA缓冲），将接收缓冲区改为双缓冲区，未测试
2025-10-15 02:10:02 +00:00
完成发送接收框架
2025-10-14 02:48:50 +00:00
#ifdef PTZ_SERVO_MOTOR
//电机数据解析任务互斥量
BSP_OS_SEM g_horiMotorMutex;
BSP_OS_SEM g_vertMotorMutex;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
/**
* @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:读取返回错误
*/
修复电机通信bug
2025-10-18 05:35:20 +00:00
static bool MotorReplyForRead(uint8_t motorNo)
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
{
修复电机通信bug
2025-10-18 05:35:20 +00:00
int16_t speed = 0;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
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;//返回数据的长度不对
修复电机通信bug
2025-10-18 05:35:20 +00:00
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
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;//返回数据的长度不对
修复电机通信bug
2025-10-18 05:35:20 +00:00
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
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;//返回数据的长度不对
修复电机通信bug
2025-10-18 05:35:20 +00:00
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
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;//返回数据的长度不对
修复电机通信bug
2025-10-18 05:35:20 +00:00
speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
if (speed < 0)
{
speed = -speed;
}
g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
servoLinkListMemPut(vertMotorType);
}
else
{
return false;
}
}
// servoLinkListMemPut(vertMotorType);
}
return true;
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
/*!
\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);
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
stopTimeOut(H_MOTOR);
修复电机通信bug
2025-10-18 05:35:20 +00:00
if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
修复错误，现在水平接收电机返回的数据完全没有问题
2025-10-16 08:35:35 +00:00
{
修复电机通信bug
2025-10-18 05:35:20 +00:00
// H_MOTOR_STOP;
continue;
修复错误，现在水平接收电机返回的数据完全没有问题
2025-10-16 08:35:35 +00:00
}
修复电机通信bug
2025-10-18 05:35:20 +00:00
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedMutex);
完成发送接收框架
2025-10-14 02:48:50 +00:00
}
}
/*!
\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);
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
stopTimeOut(V_MOTOR);
修复电机通信bug
2025-10-18 05:35:20 +00:00
if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false)
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
{
修复电机通信bug
2025-10-18 05:35:20 +00:00
// V_MOTOR_STOP;
continue;
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
}
修复电机通信bug
2025-10-18 05:35:20 +00:00
修复bug，现在水平垂直发送数据，解析数据都没有问题
2025-10-17 08:38:15 +00:00
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedMutex);
完成发送接收框架
2025-10-14 02:48:50 +00:00
}
}
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],
发送功能正常
2025-10-15 03:26:33 +00:00
(INT32U ) TASK_RECV_HORI_SERVO_STK_SIZE,
完成发送接收框架
2025-10-14 02:48:50 +00:00
(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)
{
修复错误，现在水平接收电机返回的数据完全没有问题
2025-10-16 08:35:35 +00:00
CPU_INT08U err;
完成发送接收框架
2025-10-14 02:48:50 +00:00
g_horiMotorMutex = OSSemCreate(1);
g_vertMotorMutex = OSSemCreate(1);
修复错误，现在水平接收电机返回的数据完全没有问题
2025-10-16 08:35:35 +00:00
OSSemPend(g_horiMotorMutex, 1, &err);
OSSemPend(g_vertMotorMutex, 1, &err);
完成发送接收框架
2025-10-14 02:48:50 +00:00
creat_task_servo_recv_task();
}
#endif