99 lines
3.6 KiB
C
99 lines
3.6 KiB
C
|
/*
|
|||
|
|
命令接收任务。
|
|||
|
|
|
|||
|
|
接收机器人的控制指令。
|
|||
|
|
|
|||
|
|
从DR16中接收数据，转换为通用的CMD_RC_t控制信号。
|
|||
|
|
根据CMD_RC_t计算最终命令CMD_t。
|
|||
|
|
把计算好的CMD_t细分后放到对应的消息队列中。
|
|||
|
|
超时未收到则认为是丢控等特殊情况，把CMD_RC_t中的内容置零，
|
|||
|
|
在后续的CMD_Parse中会根据此内容发现错误，保证机器人不失控。
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
/* Includes ----------------------------------------------------------------- */
|
|||
|
|
#include <string.h>
|
|||
|
|
|
|||
|
|
#include "device\dr16.h"
|
|||
|
|
#include "task\user_task.h"
|
|||
|
|
|
|||
|
|
/* Private typedef ---------------------------------------------------------- */
|
|||
|
|
/* Private define ----------------------------------------------------------- */
|
|||
|
|
/* Private macro ------------------------------------------------------------ */
|
|||
|
|
/* Private variables -------------------------------------------------------- */
|
|||
|
|
#ifdef DEBUG
|
|||
|
|
CMD_RC_t rc;
|
|||
|
|
CMD_Host_t host;
|
|||
|
|
CMD_t cmd;
|
|||
|
|
#else
|
|||
|
|
static CMD_RC_t rc;
|
|||
|
|
static CMD_Host_t host;
|
|||
|
|
static CMD_t cmd;
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/* Private function --------------------------------------------------------- */
|
|||
|
|
/* Exported functions ------------------------------------------------------- */
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* \brief 控制指令接收
|
|||
|
|
*
|
|||
|
|
* \param argument 未使用
|
|||
|
|
*/
|
|||
|
|
void Task_Command(void *argument) {
|
|||
|
|
(void)argument; /* 未使用argument，消除警告 */
|
|||
|
|
|
|||
|
|
/* 计算任务运行到指定频率需要等待的tick数 */
|
|||
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_COMMAND;
|
|||
|
|
|
|||
|
|
/* 初始化指令处理 */
|
|||
|
|
CMD_Init(&cmd, &(task_runtime.cfg.pilot_cfg->param));
|
|||
|
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
|||
|
|
|
|||
|
|
/* 用于计算遥控器数据频率 */
|
|||
|
|
while (1) {
|
|||
|
|
#ifdef DEBUG
|
|||
|
|
/* 记录任务所使用的的栈空间 */
|
|||
|
|
task_runtime.stack_water_mark.command =
|
|||
|
|
osThreadGetStackSpace(osThreadGetId());
|
|||
|
|
#endif
|
|||
|
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
|||
|
|
|
|||
|
|
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
|
|||
|
|
|
|||
|
|
/* 将接收机数据解析为指令数据 */
|
|||
|
|
if (osMessageQueueGet(task_runtime.msgq.cmd.raw.rc, &rc, 0, 0) == osOK)
|
|||
|
|
CMD_ParseRc(&rc, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
|
|||
|
|
|
|||
|
|
/* 判断是否需要让上位机覆写指令 */
|
|||
|
|
if (CMD_CheckHostOverwrite(&cmd))
|
|||
|
|
if (osMessageQueueGet(task_runtime.msgq.cmd.raw.host, &host, 0, 0) ==
|
|||
|
|
osOK)
|
|||
|
|
CMD_ParseHost(&host, &cmd, 1.0f / (float)TASK_FREQ_CTRL_COMMAND);
|
|||
|
|
|
|||
|
|
osKernelUnlock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
|
|||
|
|
|
|||
|
|
/* 将需要与其他任务分享的数据放到消息队列中 */
|
|||
|
|
osMessageQueueReset(task_runtime.msgq.cmd.ai);
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.cmd.ai, &(cmd.ai_status), 0, 0);
|
|||
|
|
|
|||
|
|
osMessageQueueReset(task_runtime.msgq.cmd.chassis);
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.cmd.chassis, &(cmd.chassis), 0, 0);
|
|||
|
|
|
|||
|
|
osMessageQueueReset(task_runtime.msgq.cmd.gimbal);
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.cmd.gimbal, &(cmd.gimbal), 0, 0);
|
|||
|
|
|
|||
|
|
osMessageQueueReset(task_runtime.msgq.cmd.shoot);
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.cmd.shoot, &(cmd.shoot), 0, 0);
|
|||
|
|
|
|||
|
|
osMessageQueueReset(task_runtime.msgq.ui.cmd);
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.ui.cmd, &cmd.pc_ctrl, 0, 0);
|
|||
|
|
|
|||
|
|
/* 存在裁判系统发送命令时，将相应的画图命令放入消息队列中 */
|
|||
|
|
while (cmd.referee.counter > 0) {
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.cmd.referee,
|
|||
|
|
&(cmd.referee.cmd[--cmd.referee.counter]), 0, 0);
|
|||
|
|
cmd.referee.cmd[cmd.referee.counter] = CMD_UI_NOTHING;
|
|||
|
|
}
|
|||
|
|
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
|||
|
|
}
|
|||
|
|
}
|