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

Merge branch 'main' into servoMotorV0.3 

# Conflicts:
#	APP/Device/Device_speed/speed_to_servoMotor.c
#	BSP/Driver/servoMotor/motorCommu.c
第一次合并
This commit is contained in:
REASEARCHER\18383 2025-10-16 10:52:20 +08:00
parent a2bbae0271 fdba688de4
commit d5ee07278f
2 changed files with 202 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,14 +9,11 @@ BSP_OS_SEM g_vertSpeedSem;
BSP_OS_SEM g_horiSpeedMutex; BSP_OS_SEM g_horiSpeedMutex;
BSP_OS_SEM g_vertSpeedMutex; BSP_OS_SEM g_vertSpeedMutex;
uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0}; uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0};
OS_MEM *g_memPtr; OS_MEM *g_memPtr;
ptzServoLinkList g_servoMotorLinkList = {0}; ptzServoLinkList g_servoMotorLinkList = {0};
//发送云台实际转速 //发送云台实际转速
void ptz_send_speed(char dev, char speed) void ptz_send_speed(char dev, char speed)
{ {
@ -62,17 +59,72 @@ void ptz_send_speed(char dev, char speed)
\param[out] none \param[out] none
\retval none \retval none
*/ */
void servoLinkListMemPut(linkList *data) void servoHoriLinkListMemPut(uint8_t motorType)
{ {
if (data == NULL) { //水平电机
return; if (motorType == horiMotorType) {
} //当前发送完成的数据为高优先级链表中的数据
if (g_servoMotorLinkList.horiMotor.linkListNum == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
linkList *ptr; linkList *ptr;
ptr = data; ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
data = data->next; if (ptr->next != NULL) {
g_servoMotorLinkList.horiMotor.LinkListHead_H = ptr->next;
}
else {
g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
}
OSMemPut(g_memPtr, (uint8_t *)ptr); OSMemPut(g_memPtr, (uint8_t *)ptr);
} }
}
//当前发送完成的数据为低优先级链表中的数据
else if (g_servoMotorLinkList.horiMotor.linkListNum == lowPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L;
if (ptr->next != NULL) {
g_servoMotorLinkList.horiMotor.LinkListHead_L = ptr->next;
}
else {
g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
}
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
}
else if (motorType == vertMotorType) {
if (g_servoMotorLinkList.vertMotor.linkListNum == highPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H;
if (ptr->next != NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_H = ptr->next;
}
else {
g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
}
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
else if (g_servoMotorLinkList.vertMotor.linkListNum == lowPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
linkList *ptr;
ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L;
if (ptr->next != NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_L = ptr->next;
}
else {
g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
}
OSMemPut(g_memPtr, (uint8_t *)ptr);
}
}
}
}
/*! /*!
\brief 发送函数 \brief 发送函数
@ -119,14 +171,28 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
} }
if (priority == highPriority) { if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListTail_H == NULL) {
g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr; g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
}
else {
g_servoMotorLinkList.horiMotor.LinkListTail_H->next = ptr;
g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
}
if (g_servoMotorLinkList.horiMotor.LinkListHead_H == NULL) { if (g_servoMotorLinkList.horiMotor.LinkListHead_H == NULL) {
g_servoMotorLinkList.horiMotor.LinkListHead_H g_servoMotorLinkList.horiMotor.LinkListHead_H
= g_servoMotorLinkList.horiMotor.LinkListTail_H; = g_servoMotorLinkList.horiMotor.LinkListTail_H;
} }
} }
else { else {
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr; g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
}
else {
g_servoMotorLinkList.horiMotor.LinkListTail_L->next = ptr;
g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
}
if (g_servoMotorLinkList.horiMotor.LinkListHead_L == NULL) { if (g_servoMotorLinkList.horiMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.horiMotor.LinkListHead_L g_servoMotorLinkList.horiMotor.LinkListHead_L
= g_servoMotorLinkList.horiMotor.LinkListTail_L; = g_servoMotorLinkList.horiMotor.LinkListTail_L;
@ -143,14 +209,28 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
} }
if (priority == highPriority) { if (priority == highPriority) {
if (g_servoMotorLinkList.vertMotor.LinkListTail_H == NULL) {
g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr; g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
}
else {
g_servoMotorLinkList.vertMotor.LinkListTail_H->next = ptr;
g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
}
if (g_servoMotorLinkList.vertMotor.LinkListHead_H == NULL) { if (g_servoMotorLinkList.vertMotor.LinkListHead_H == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_H g_servoMotorLinkList.vertMotor.LinkListHead_H
= g_servoMotorLinkList.vertMotor.LinkListTail_H; = g_servoMotorLinkList.vertMotor.LinkListTail_H;
} }
} }
else { else {
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr; g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
}
else {
g_servoMotorLinkList.vertMotor.LinkListTail_L->next = ptr;
g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
}
if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) { if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
g_servoMotorLinkList.vertMotor.LinkListHead_L g_servoMotorLinkList.vertMotor.LinkListHead_L
= g_servoMotorLinkList.vertMotor.LinkListTail_L; = g_servoMotorLinkList.vertMotor.LinkListTail_L;
@ -289,6 +369,102 @@ static void creat_task_vert_servo_task(void)
} }
} }
void horiServoTimeOut()
{
OSSemPost(g_horiSpeedMutex);
}
void vertServoTimeOut()
{
OSSemPost(g_vertSpeedMutex);
}
BSP_OS_TMR horiServoSoftWareTim;
BSP_OS_TMR vertServoSoftWareTim;
void servoCommSoftWareTimInit()
{
CPU_INT08U ServoSoftWareTimErr;
horiServoSoftWareTim = OSTmrCreate(10
, 10
, OS_TMR_OPT_PERIODIC
, (OS_TMR_CALLBACK)horiServoTimeOut
, (void *)0
, "tmr1"
, &ServoSoftWareTimErr);
if ((ServoSoftWareTimErr == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create horiServoSoftWareTim success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim failed...\n\r");
}
vertServoSoftWareTim = OSTmrCreate(10
, 10
, OS_TMR_OPT_PERIODIC
, (OS_TMR_CALLBACK)vertServoTimeOut
, (void *)0
, "tmr2"
, &ServoSoftWareTimErr);
if ((ServoSoftWareTimErr == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create vertServoSoftWareTim success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create vertServoSoftWareTim failed...\n\r");
}
}
/*!
\brief 停止软件定时器
\param[in] motor：电机
horiMotorType 水平电机
vertMotorType 俯仰电机
\param[out] none
\retval none
*/
void stopTimeOut(uint8_t motorType)
{
if (motorType == horiMotorType) {
CPU_INT08U err;
OSTmrStop(horiServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err);
if (err != OS_ERR_NONE) {
pdebug(DEBUG_LEVEL_FATAL,"start horiServoSoftWareTim failed...\n\r");
}
}
else if (motorType == vertMotorType) {
CPU_INT08U err;
OSTmrStop(vertServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err);
if (err != OS_ERR_NONE) {
pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r");
}
}
}
/*!
\brief 启动软件定时器
\param[in] motor：电机
horiMotorType 水平电机
vertMotorType 俯仰电机
\param[out] none
\retval none
*/
void startTimeOut(uint8_t motorType)
{
if (motorType == horiMotorType) {
CPU_INT08U err;
OSTmrStart(horiServoSoftWareTim, &err);
if (err != OS_ERR_NONE) {
pdebug(DEBUG_LEVEL_FATAL,"start horiServoSoftWareTim failed...\n\r");
}
}
else if (motorType == vertMotorType) {
CPU_INT08U err;
OSTmrStart(vertServoSoftWareTim, &err);
if (err != OS_ERR_NONE) {
pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim failed...\n\r");
}
}
}
void init_speed_module(void) void init_speed_module(void)
{ {
g_horiSpeedSem = OSSemCreate(0); g_horiSpeedSem = OSSemCreate(0);
@ -325,12 +501,15 @@ void init_speed_module(void)
creat_task_hori_servo_task(); creat_task_hori_servo_task();
creat_task_vert_servo_task(); creat_task_vert_servo_task();
// creat_task_test();
servoCommSoftWareTimInit();
OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功 OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); uint8_t buffer[20] = {0};
uint8_t buffer_len = 0;
buffer_len = WriteMotorOneReg_buffer(H_MOTOR, H02_CONTR_MODE_SELEC, 0, buffer);
servoSendData(horiMotorType, buffer, buffer_len, lowPriority);
} }

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

@ -67,8 +67,10 @@ 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 servoLinkListMemPut(linkList *data);
void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority); void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
void servoHoriLinkListMemPut(uint8_t motorType);
void stopTimeOut(uint8_t motorType);
void startTimeOut(uint8_t motorType);
#endif #endif