pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完成发送接收框架

This commit is contained in:
起床就犯困 2025-10-14 10:48:50 +08:00
parent 1a2be27fb9
commit bd1d26de10
10 changed files with 428 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@ -45,6 +45,7 @@
"angle_poweroffsave.h": "c", "angle_poweroffsave.h": "c",
"comm_cfginfo.h": "c", "comm_cfginfo.h": "c",
"servomotor.h": "c", "servomotor.h": "c",
"pdebug.h": "c" "pdebug.h": "c",
"limits": "c"
} }
} }

10
APP/Appcfg/app_cfg.h
View File

@ -145,6 +145,16 @@
#define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u #define TASK_PTZ_TASK_PRINTF_STK_SIZE 200u
//解析水平电机返回的数据
#define TASK_RECV_HORI_SERVO_PRIO 56u
#define TASK_RECV_HORI_SERVO_STK_SIZE 200u
//解析俯仰电机返回的数据
#define TASK_RECV_VERT_SERVO_PRIO 57u
#define TASK_RECV_VERT_SERVO_STK_SIZE 200u
//#define ????????????????????_PRIO 55u //#define ????????????????????_PRIO 55u
//#define ????????????????????_STK_SIZE 150u //#define ????????????????????_STK_SIZE 150u
/*******************************************************************************/ /*******************************************************************************/

103
APP/Device/Device_speed/servoMotor_recv.c Normal file
View File

@ -0,0 +1,103 @@
#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[in] none
\param[out] none
\retval none
*/
static void ptz_recv_hori_servo_task()
{
CPU_INT08U err;
while(1) {
OSSemPend(g_horiMotorMutex, 0, &err);
}
}
/*!
\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);
}
}
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_vert_servo_stk,
(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)
{
g_horiMotorMutex = OSSemCreate(1);
g_vertMotorMutex = OSSemCreate(1);
creat_task_servo_recv_task();
}
#endif

15
APP/Device/Device_speed/servoMotor_recv.h Normal file
View File

@ -0,0 +1,15 @@
#ifndef __DEVICE_SPEED_SERVOMOTOR_H_
#define __DEVICE_SPEED_SERVOMOTOR_H_
#include "ptz_type_select.h"
#ifdef PTZ_SERVO_MOTOR
void Init_ServoMotorRecv(void);
#endif
#endif

263
APP/Device/Device_speed/speed_to_servoMotor.c
View File

@ -1,9 +1,21 @@
#include "speed_to_servoMotor.h" #include "speed_to_servoMotor.h"
#include "ptz_header_file.h"
#include "servoMotor.h"
#ifdef PTZ_SERVO_MOTOR #ifdef PTZ_SERVO_MOTOR
//云台伺服电机通信信号量，增加发送的数据信号量+1，发送完成信号量-1
BSP_OS_SEM g_horiSpeedSem;
BSP_OS_SEM g_vertSpeedSem;
//云台伺服电机能否发生数据互斥量
BSP_OS_SEM g_horiSpeedMutex;
BSP_OS_SEM g_vertSpeedMutex;
uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0};
OS_MEM *g_memPtr;
ptzServoLinkList g_servoMotorLinkList = {0};
//发送云台实际转速 //发送云台实际转速
void ptz_send_speed(char dev, char speed) void ptz_send_speed(char dev, char speed)
@ -44,51 +56,195 @@ void ptz_send_speed(char dev, char speed)
} }
} }
/*!
static void ptz_hori_step_speed_task() \brief 链表发送完成后用于释放内存
\param[in] data：储存发送数据的链表
\param[out] none
\retval none
*/
void servoLinkListMemPut(linkList *data)
{ {
while(1) if (data == NULL) {
{ return;
if(g_ptz.hori_start_stop_set == PTZ_HORI_START) }
{
linkList *ptr;
ptr = data;
data = data->next;
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
/*!
\brief 发送函数
\param[in] motor：电机
horiMotor 水平电机
vertMotor 俯仰电机
\param[in] *data：要发送的数据
\param[in] dataLen：指向要发送的数据
\param[in] priority：将要发送的数据填入优先级
\param[out] none
\retval none
*/
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
{
if ((motor != horiMotorType) && (motor!= vertMotorType)) {
return;
}
if (data == NULL) {
return;
}
if (dataLen > sendDataBufLen) {
return;
}
if ((priority != highPriority) && (priority != lowPriority)) {
return;
}
linkList *ptr = NULL;
CPU_INT08U err;
//保存数据到链表节点
ptr = OSMemGet(g_memPtr, &err);
ptr->length = dataLen;
memcpy(ptr->data, data, dataLen);
//将节点添加进入链表中
if (motor == horiMotorType) {
if (g_servoMotorLinkList.horiMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return;
} }
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) if (priority == highPriority) {
{ g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
if (g_servoMotorLinkList.horiMotor.LinkListHead_H == NULL) {
g_servoMotorLinkList.horiMotor.LinkListHead_H
= g_servoMotorLinkList.horiMotor.LinkListTail_H;
}
}
else {
g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
if (g_servoMotorLinkList.horiMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.horiMotor.LinkListHead_L
= g_servoMotorLinkList.horiMotor.LinkListTail_L;
}
}
//释放信号量，通知能发送一次
OSSemPost(g_horiSpeedSem);
}
else {
if (g_servoMotorLinkList.vertMotor.linkListNum > sendDataBufNumber) {
OSMemPut(g_memPtr, ptr);
return;
}
if (priority == highPriority) {
g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
if (g_servoMotorLinkList.vertMotor.LinkListHead_H == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_H
= g_servoMotorLinkList.vertMotor.LinkListTail_H;
}
}
else {
g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_L
= g_servoMotorLinkList.vertMotor.LinkListTail_L;
}
}
//释放信号量，通知能发送一次
OSSemPost(g_vertSpeedSem);
}
}
static void ptz_hori_servo_task()
{
CPU_INT08U err;
while(1) {
// if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
// {
// }
// if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
// {
// }
OSSemPend(g_horiSpeedMutex, 0, &err);
OSSemPend(g_horiSpeedSem, 0, &err);
// 高优先级链表中数据先发送
if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
//发送数据
g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
continue;
}
// 高优先级链表中无数据时，发送低优先级中的数据
if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
//发送数据
g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task no data...\n\r");
} }
OSTimeDlyHMSM(0u, 0u, 0u, 10);
} }
} }
static void ptz_vert_step_speed_task() static void ptz_vert_servo_task()
{ {
while(1) CPU_INT08U err;
{ while(1) {
if(g_ptz.vert_start_stop_set == PTZ_VERT_START) // if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{ // {
// }
// if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
// {
// }
OSSemPend(g_vertSpeedMutex, 0, &err);
OSSemPend(g_vertSpeedSem, 0, &err);
// 高优先级链表中数据先发送
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
continue;
} }
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
{ // 高优先级链表中无数据时，发送低优先级中的数据
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task no data...\n\r");
} }
OSTimeDlyHMSM(0u, 0u, 0u, 10);
} }
} }
static OS_STK task_hori_servo_stk[TASK_HORI_PID_STK_SIZE];
static void creat_task_hori_servo_task(void)
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 task_err;
CPU_INT08U name_err; CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_step_speed_task, task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_servo_task,
(void *) 0, (void *) 0,
(OS_STK *)&task_hori_step_speed_stk[TASK_HORI_PID_STK_SIZE - 1], (OS_STK *)&task_hori_servo_stk[TASK_HORI_PID_STK_SIZE - 1],
(INT8U ) TASK_HORI_PID_PRIO, (INT8U ) TASK_HORI_PID_PRIO,
(INT16U ) TASK_HORI_PID_PRIO, (INT16U ) TASK_HORI_PID_PRIO,
(OS_STK *)&task_hori_step_speed_stk[0], (OS_STK *)&task_hori_servo_stk[0],
(INT32U ) TASK_HORI_PID_STK_SIZE, (INT32U ) TASK_HORI_PID_STK_SIZE,
(void *) 0, (void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
@ -104,46 +260,75 @@ static void creat_task_hori_step_speed_task(void)
} }
static OS_STK task_vert_servo_stk[TASK_VERT_PID_STK_SIZE];
static OS_STK task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE]; static void creat_task_vert_servo_task(void)
static void creat_task_vert_step_speed_task(void)
{ {
CPU_INT08U task_err; CPU_INT08U task_err;
CPU_INT08U name_err; CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_step_speed_task, task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_servo_task,
(void *) 0, (void *) 0,
(OS_STK *)&task_vert_step_speed_stk[TASK_VERT_PID_STK_SIZE - 1], (OS_STK *)&task_vert_servo_stk[TASK_VERT_PID_STK_SIZE - 1],
(INT8U ) TASK_VERT_PID_PRIO, (INT8U ) TASK_VERT_PID_PRIO,
(INT16U ) TASK_VERT_PID_PRIO, (INT16U ) TASK_VERT_PID_PRIO,
(OS_STK *)&task_vert_step_speed_stk[0], (OS_STK *)&task_vert_servo_stk[0],
(INT32U ) TASK_VERT_PID_STK_SIZE, (INT32U ) TASK_VERT_PID_STK_SIZE,
(void *) 0, (void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0) #if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_step_speed_task", &name_err); OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_servo_task", &name_err);
#endif #endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_step_speed_task success...\n\r"); pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_servo_task success...\n\r");
} else { } else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_step_speed_task failed...\n\r"); pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_servo_task failed...\n\r");
} }
} }
void init_speed_module(void) void init_speed_module(void)
{ {
g_horiSpeedSem = OSSemCreate(0);
g_vertSpeedSem = OSSemCreate(0);
g_horiSpeedMutex = OSSemCreate(1);
g_vertSpeedMutex = OSSemCreate(1);
OSSemPost(g_horiSpeedMutex);
OSSemPost(g_vertSpeedMutex);
CPU_INT08U err;
g_memPtr = OSMemCreate(memBuffer, sendDataBufNumber * 2, linkListNodeLen, &err);
if (err != OS_ERR_NONE) {
pdebug(DEBUG_LEVEL_FATAL, "Failed to create the motor memory pool\n\r");
}
//初始化链表头尾
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
g_servoMotorLinkList.horiMotor.linkListPriority = nonePriority;
g_servoMotorLinkList.horiMotor.linkListNum = 0;
g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
g_servoMotorLinkList.vertMotor.linkListPriority = nonePriority;
g_servoMotorLinkList.vertMotor.linkListNum = 0;
servoMotorInit(); servoMotorInit();
creat_task_hori_step_speed_task(); Init_ServoMotorRecv();
creat_task_vert_step_speed_task();
creat_task_hori_servo_task();
creat_task_vert_servo_task();
} }
#endif #endif

66
APP/Device/Device_speed/speed_to_servoMotor.h
View File

@ -1,13 +1,75 @@
#ifndef __DEVICE_SPEED_SERVOMOTOR_H_ #ifndef __DEVICE_SPEED_SERVOMOTOR_H_
#define __DEVICE_SPEED_SERVOMOTOR_H_ #define __DEVICE_SPEED_SERVOMOTOR_H_
#include "ptz_type_select.h" #include "ptz_type_select.h"
#include "stdint.h"
#include "ptz_header_file.h"
#include "servoMotor.h"
#include "servoMotor_recv.h"
#ifdef PTZ_SERVO_MOTOR #ifdef PTZ_SERVO_MOTOR
enum {
nonePriority = 0,
highPriority,
lowPriority,
};
enum {
horiMotorType = 0,
vertMotorType,
};
//发送数据缓冲区的个数
#define sendDataBufNumber 8
//发送数据缓冲区的长度
#define sendDataBufLen 25
//发送链表，当个节点最大大小
#define linkListNodeLen 50
//发送链表
typedef struct _linkList {
//发送的数据
uint8_t data[sendDataBufLen];
//发送的数据长度
uint16_t length;
//下一个节点
struct _linkList *next;
} linkList;
typedef struct _motorLinkList{
//发送数据，高优先级链表头
linkList *LinkListHead_H;
//发送数据，低优先级链表头
linkList *LinkListHead_L;
//水平电机发送数据，高优先级链表尾
linkList *LinkListTail_H;
//水平电机发送数据，低优先级链表尾
linkList *LinkListTail_L;
//当前发送数据的链表优先级
uint8_t linkListPriority;
//放入的数据个数
uint8_t linkListNum;
} motorLinkList;
typedef struct _ptzServoLinkList {
motorLinkList horiMotor;
motorLinkList vertMotor;
} ptzServoLinkList;
extern BSP_OS_SEM g_horiSpeedSem;
extern BSP_OS_SEM g_vertSpeedSem;
extern BSP_OS_SEM g_horiSpeedMutex;
extern BSP_OS_SEM g_vertSpeedMutex;
extern ptzServoLinkList g_servoMotorLinkList;
void ptz_send_speed(char dev, char speed); void ptz_send_speed(char dev, char speed);
void init_speed_module(void); void init_speed_module(void);
void init_speed_module(void);
void servoLinkListMemPut(linkList *data);
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
#endif #endif
#endif #endif

1
BSP/Driver/servoMotor/servoMotor.c
View File

@ -61,6 +61,7 @@ void servoMotorInit(void)
{ {
MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要 MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
H_MOTOR_OPEN; H_MOTOR_OPEN;
OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
V_MOTOR_OPEN; V_MOTOR_OPEN;
CommuDrvInit();//伺服电机RS485通讯初始化 CommuDrvInit();//伺服电机RS485通讯初始化

3
PROJECT/OS2.ewp
View File

@ -2435,6 +2435,9 @@
<file> <file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_step.h</name> <name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_step.h</name>
</file> </file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\servoMotor_recv.c</name>
</file>
<file> <file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_bldc.c</name> <name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_bldc.c</name>
<configuration> <configuration>

3
PROJECT/OS2.ewt
View File

@ -3011,6 +3011,9 @@
<file> <file>
<name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_step.h</name> <name>$PROJ_DIR$\..\APP\Device\Device_rotate\rotate_step.h</name>
</file> </file>
<file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\servoMotor_recv.c</name>
</file>
<file> <file>
<name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_bldc.c</name> <name>$PROJ_DIR$\..\APP\Device\Device_speed\speed_to_bldc.c</name>
<configuration> <configuration>