94 lines
3.2 KiB
C
94 lines
3.2 KiB
C
|
/*
|
||
|
|
CAN总线数据处理
|
||
|
|
|
||
|
|
处理CAN总线收到的电机电容数据。
|
||
|
|
*/
|
||
|
|
|
||
|
|
/* Includes ----------------------------------------------------------------- */
|
||
|
|
#include "device\can.h"
|
||
|
|
|
||
|
|
#include "device\referee.h"
|
||
|
|
#include "task\user_task.h"
|
||
|
|
|
||
|
|
/* Private typedef ---------------------------------------------------------- */
|
||
|
|
/* Private define ----------------------------------------------------------- */
|
||
|
|
/* Private macro ------------------------------------------------------------ */
|
||
|
|
/* Private variables -------------------------------------------------------- */
|
||
|
|
|
||
|
|
#ifdef DEBUG
|
||
|
|
CAN_t can;
|
||
|
|
CAN_Output_t can_out;
|
||
|
|
CAN_RawRx_t can_rx;
|
||
|
|
#else
|
||
|
|
static CAN_t can;
|
||
|
|
static CAN_Output_t can_out;
|
||
|
|
static CAN_RawRx_t can_rx;
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* Private function --------------------------------------------------------- */
|
||
|
|
/* Exported functions ------------------------------------------------------- */
|
||
|
|
void Task_Can(void *argument) {
|
||
|
|
(void)argument;
|
||
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
||
|
|
|
||
|
|
/* Device Setup */
|
||
|
|
CAN_Init(&can, &task_runtime.cfg.robot_param->can);
|
||
|
|
|
||
|
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||
|
|
/* Task Setup */
|
||
|
|
while (1) {
|
||
|
|
#ifdef DEBUG
|
||
|
|
task_runtime.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
|
||
|
|
#endif
|
||
|
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
|
|
while (osMessageQueueGet(can.msgq_raw, &can_rx, 0, 0) == osOK) {
|
||
|
|
CAN_StoreMsg(&can, &can_rx);
|
||
|
|
}
|
||
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.chassis);
|
||
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.chassis, &can, 0, 0);
|
||
|
|
|
||
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.gimbal);
|
||
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.gimbal, &can, 0, 0);
|
||
|
|
|
||
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.shoot);
|
||
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.shoot, &can, 0, 0);
|
||
|
|
|
||
|
|
if (CAN_CheckFlag(&can, CAN_REC_CAP_FINISHED)) {
|
||
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.cap);
|
||
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.cap, &can, 0, 0);
|
||
|
|
CAN_ClearFlag(&can, CAN_REC_CAP_FINISHED);
|
||
|
|
} else {
|
||
|
|
// Error Handle
|
||
|
|
}
|
||
|
|
|
||
|
|
if (CAN_CheckFlag(&can, CAN_REC_TOF_FINISHED)) {
|
||
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.tof);
|
||
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.tof, &can, 0, 0);
|
||
|
|
CAN_ClearFlag(&can, CAN_REC_TOF_FINISHED);
|
||
|
|
} else {
|
||
|
|
// Error Handle
|
||
|
|
}
|
||
|
|
|
||
|
|
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis,
|
||
|
|
&(can_out.chassis), 0, 0) == osOK) {
|
||
|
|
CAN_Motor_Control(CAN_MOTOR_GROUT_CHASSIS, &can_out, &can);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (osMessageQueueGet(task_runtime.msgq.can.output.gimbal,
|
||
|
|
&(can_out.gimbal), 0, 0) == osOK) {
|
||
|
|
CAN_Motor_Control(CAN_MOTOR_GROUT_GIMBAL1, &can_out, &can);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (osMessageQueueGet(task_runtime.msgq.can.output.shoot, &(can_out.shoot),
|
||
|
|
0, 0) == osOK) {
|
||
|
|
CAN_Motor_Control(CAN_MOTOR_GROUT_SHOOT1, &can_out, &can);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (osMessageQueueGet(task_runtime.msgq.can.output.cap, &(can_out.cap), 0,
|
||
|
|
0) == osOK) {
|
||
|
|
CAN_Cap_Control(&(can_out.cap), &can);
|
||
|
|
}
|
||
|
|
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
|
|
}
|
||
|
|
}
|