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_vertSpeedMutex;
uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0};
OS_MEM *g_memPtr;
ptzServoLinkList g_servoMotorLinkList = {0};
//发送云台实际转速
void ptz_send_speed(char dev, char speed)
{
@ -62,16 +59,71 @@ void ptz_send_speed(char dev, char speed)
\param[out] 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;
ptr = data;
data = data->next;
ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
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);
}
}
//当前发送完成的数据为低优先级链表中的数据
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);
}
}
}
}
/*!
@ -119,14 +171,28 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
}
if (priority == highPriority) {
if (g_servoMotorLinkList.horiMotor.LinkListTail_H == NULL) {
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) {
g_servoMotorLinkList.horiMotor.LinkListHead_H
= g_servoMotorLinkList.horiMotor.LinkListTail_H;
}
}
else {
if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
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) {
g_servoMotorLinkList.horiMotor.LinkListHead_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 (g_servoMotorLinkList.vertMotor.LinkListTail_H == NULL) {
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) {
g_servoMotorLinkList.vertMotor.LinkListHead_H
= g_servoMotorLinkList.vertMotor.LinkListTail_H;
}
}
else {
if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
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) {
g_servoMotorLinkList.vertMotor.LinkListHead_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)
{
g_horiSpeedSem = OSSemCreate(0);
@ -325,12 +501,15 @@ void init_speed_module(void)
creat_task_hori_servo_task();
creat_task_vert_servo_task();
// creat_task_test();
servoCommSoftWareTimInit();
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 init_speed_module(void);
void servoLinkListMemPut(linkList *data);
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