98 lines
3.6 KiB
C
98 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); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
}
|