335 lines
10 KiB
C
335 lines
10 KiB
C
#include "speed_to_servoMotor.h"
|
||
|
||
#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)
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
|
||
/*!
|
||
\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;
|
||
}
|
||
|
||
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 (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");
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
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");
|
||
}
|
||
}
|
||
}
|
||
|
||
static OS_STK task_hori_servo_stk[TASK_HORI_PID_STK_SIZE];
|
||
static void creat_task_hori_servo_task(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_servo_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_hori_servo_stk[TASK_HORI_PID_STK_SIZE - 1],
|
||
(INT8U ) TASK_HORI_PID_PRIO,
|
||
(INT16U ) TASK_HORI_PID_PRIO,
|
||
(OS_STK *)&task_hori_servo_stk[0],
|
||
(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");
|
||
}
|
||
}
|
||
|
||
|
||
static OS_STK task_vert_servo_stk[TASK_VERT_PID_STK_SIZE];
|
||
static void creat_task_vert_servo_task(void)
|
||
{
|
||
CPU_INT08U task_err;
|
||
CPU_INT08U name_err;
|
||
|
||
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_servo_task,
|
||
(void *) 0,
|
||
(OS_STK *)&task_vert_servo_stk[TASK_VERT_PID_STK_SIZE - 1],
|
||
(INT8U ) TASK_VERT_PID_PRIO,
|
||
(INT16U ) TASK_VERT_PID_PRIO,
|
||
(OS_STK *)&task_vert_servo_stk[0],
|
||
(INT32U ) TASK_VERT_PID_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_vert_servo_task", &name_err);
|
||
#endif
|
||
|
||
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
|
||
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_servo_task success...\n\r");
|
||
} else {
|
||
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_servo_task failed...\n\r");
|
||
}
|
||
}
|
||
|
||
|
||
|
||
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();
|
||
Init_ServoMotorRecv();
|
||
|
||
creat_task_hori_servo_task();
|
||
creat_task_vert_servo_task();
|
||
}
|
||
|
||
|
||
|
||
|
||
#endif
|
||
|
||
|