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

335 lines
10 KiB
C
Raw Permalink Normal View History

修改宏定义
2025-10-11 03:40:39 +00:00
#include "speed_to_servoMotor.h"
#ifdef PTZ_SERVO_MOTOR
完成发送接收框架
2025-10-14 02:48:50 +00:00
//云台伺服电机通信信号量，增加发送的数据信号量+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};
修改宏定义
2025-10-11 03:40:39 +00:00
//发送云台实际转速
void ptz_send_speed(char dev, char speed)
{
unsigned char Speed1[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
unsigned char Speed2[7] = {0xff,0x00,0xd0,0x00,0x00,0x00,0x00};
unsigned short int HSpeed = 0;
unsigned short int VSpeed = 0;
Speed1[1] = g_ptz.address;
Speed2[1] = g_ptz.address;
HSpeed = (unsigned short int)(g_ptz.hori_speed_actual * 100 + 0.5);
Speed1[3] = 0X03;//水平电机
Speed1[4] = (unsigned char)(HSpeed >> 8);
Speed1[5] = (unsigned char)(HSpeed & 0x00ff);
Speed1[6] = MotorCalPelcoDSUM(Speed1,sizeof(Speed1));
VSpeed = (unsigned short int)(g_ptz.vert_speed_actual * 100 + 0.5);
Speed2[3] = 0X04;//垂直电机
Speed2[4] = (unsigned char)(VSpeed >> 8);
Speed2[5] = (unsigned char)(VSpeed & 0x00ff);
Speed2[6] = MotorCalPelcoDSUM(Speed2,sizeof(Speed2));
switch(speed)
{
case 1://只查询水平转速
ptz_send_data(dev,Speed1,sizeof(Speed1));
break;
case 2://只查询垂直转速
ptz_send_data(dev,Speed2,sizeof(Speed2));
break;
default://水平转速垂直转速都查询
ptz_send_data(dev,Speed1,sizeof(Speed1));
ptz_send_data(dev,Speed2,sizeof(Speed2));
break;
}
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
/*!
\brief 链表发送完成后用于释放内存
\param[in] data：储存发送数据的链表
\param[out] none
\retval none
*/
void servoLinkListMemPut(linkList *data)
{
if (data == NULL) {
return;
}
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;
}
修改宏定义
2025-10-11 03:40:39 +00:00
完成发送接收框架
2025-10-14 02:48:50 +00:00
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;
修改宏定义
2025-10-11 03:40:39 +00:00
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
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;
修改宏定义
2025-10-11 03:40:39 +00:00
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
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);
修改宏定义
2025-10-11 03:40:39 +00:00
}
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
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);
修改宏定义
2025-10-11 03:40:39 +00:00
完成发送接收框架
2025-10-14 02:48:50 +00:00
// 高优先级链表中数据先发送
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");
修改宏定义
2025-10-11 03:40:39 +00:00
}
}
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
static void ptz_vert_servo_task()
{
CPU_INT08U err;
while(1) {
// 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_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
}
else {
pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task no data...\n\r");
}
}
}
修改宏定义
2025-10-11 03:40:39 +00:00
完成发送接收框架
2025-10-14 02:48:50 +00:00
static OS_STK task_hori_servo_stk[TASK_HORI_PID_STK_SIZE];
static void creat_task_hori_servo_task(void)
修改宏定义
2025-10-11 03:40:39 +00:00
{
CPU_INT08U task_err;
CPU_INT08U name_err;
完成发送接收框架
2025-10-14 02:48:50 +00:00
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_servo_task,
修改宏定义
2025-10-11 03:40:39 +00:00
(void *) 0,
完成发送接收框架
2025-10-14 02:48:50 +00:00
(OS_STK *)&task_hori_servo_stk[TASK_HORI_PID_STK_SIZE - 1],
修改宏定义
2025-10-11 03:40:39 +00:00
(INT8U ) TASK_HORI_PID_PRIO,
(INT16U ) TASK_HORI_PID_PRIO,
完成发送接收框架
2025-10-14 02:48:50 +00:00
(OS_STK *)&task_hori_servo_stk[0],
修改宏定义
2025-10-11 03:40:39 +00:00
(INT32U ) TASK_HORI_PID_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_HORI_PID_PRIO, "ptz_hori_step_speed_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_step_speed_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_step_speed_task failed...\n\r");
}
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
static OS_STK task_vert_servo_stk[TASK_VERT_PID_STK_SIZE];
static void creat_task_vert_servo_task(void)
修改宏定义
2025-10-11 03:40:39 +00:00
{
CPU_INT08U task_err;
CPU_INT08U name_err;
完成发送接收框架
2025-10-14 02:48:50 +00:00
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_servo_task,
修改宏定义
2025-10-11 03:40:39 +00:00
(void *) 0,
完成发送接收框架
2025-10-14 02:48:50 +00:00
(OS_STK *)&task_vert_servo_stk[TASK_VERT_PID_STK_SIZE - 1],
修改宏定义
2025-10-11 03:40:39 +00:00
(INT8U ) TASK_VERT_PID_PRIO,
(INT16U ) TASK_VERT_PID_PRIO,
完成发送接收框架
2025-10-14 02:48:50 +00:00
(OS_STK *)&task_vert_servo_stk[0],
修改宏定义
2025-10-11 03:40:39 +00:00
(INT32U ) TASK_VERT_PID_STK_SIZE,
(void *) 0,
完成发送接收框架
2025-10-14 02:48:50 +00:00
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
修改宏定义
2025-10-11 03:40:39 +00:00
#if (OS_TASK_NAME_EN > 0)
完成发送接收框架
2025-10-14 02:48:50 +00:00
OSTaskNameSet(TASK_VERT_PID_PRIO, "ptz_vert_servo_task", &name_err);
修改宏定义
2025-10-11 03:40:39 +00:00
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
完成发送接收框架
2025-10-14 02:48:50 +00:00
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_servo_task success...\n\r");
修改宏定义
2025-10-11 03:40:39 +00:00
} else {
完成发送接收框架
2025-10-14 02:48:50 +00:00
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_servo_task failed...\n\r");
修改宏定义
2025-10-11 03:40:39 +00:00
}
}
void init_speed_module(void)
{
完成发送接收框架
2025-10-14 02:48:50 +00:00
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;
修改宏定义
2025-10-11 03:40:39 +00:00
servoMotorInit();
完成发送接收框架
2025-10-14 02:48:50 +00:00
Init_ServoMotorRecv();
creat_task_hori_servo_task();
creat_task_vert_servo_task();
修改宏定义
2025-10-11 03:40:39 +00:00
}
完成发送接收框架
2025-10-14 02:48:50 +00:00
修改宏定义
2025-10-11 03:40:39 +00:00
#endif