75 lines
2.1 KiB
C
75 lines
2.1 KiB
C
/*
|
|
CAN总线数据处理
|
|
|
|
处理CAN总线收到的电机数据
|
|
*/
|
|
|
|
#include "can_use.h"
|
|
#include "user_task.h"
|
|
|
|
#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
|
|
|
|
|
|
void Task_can(void *argument)
|
|
{
|
|
(void)argument;
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
|
|
|
|
/* Device Setup */
|
|
CAN_Init(&can, &(task_runtime.config.chassis_config->can));
|
|
|
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计算 */
|
|
|
|
/* Task Setup */
|
|
while (1) {
|
|
#ifdef DEBUG
|
|
task_runtime.stack_water_mark.can = osThreadGetStackSpace(osThreadGetId());
|
|
#endif
|
|
|
|
while (osMessageQueueGet(can.msgq_raw, &can_rx, 0, 0)==osOK)
|
|
{
|
|
CAN_StoreMsg(&can, &can_rx);
|
|
}
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.chassisfed6020);
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.chassisfed6020, &can, 0, 0);
|
|
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.chassisfed5065);
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.chassisfed5065, &can, 0, 0);
|
|
|
|
// osMessageQueueReset(task_runtime.msgq.can.feedback.pitchfed6020);
|
|
// osMessageQueuePut(task_runtime.msgq.can.feedback.pitchfed6020, &can, 0,0);
|
|
|
|
osMessageQueueReset(task_runtime.msgq.can.feedback.sickfed);
|
|
osMessageQueuePut(task_runtime.msgq.can.feedback.sickfed, &can, 0,0);
|
|
|
|
// if (osMessageQueueGet(task_runtime.msgq.can.output.pitch6020,
|
|
// &(can_out.pitch6020),0,0) == osOK) {
|
|
// CAN_DJIMotor_Control(CAN_MOTOR_PITCH6020,&can_out,&can);
|
|
// }
|
|
|
|
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis5065,
|
|
&(can_out.chassis5065), 0, 0) == osOK) {
|
|
CAN_VESC_Control(11,CAN_MOTOR_CHASSIS5065, &can_out ,&can);
|
|
}
|
|
|
|
if (osMessageQueueGet(task_runtime.msgq.can.output.chassis6020,
|
|
&(can_out.chassis6020), 0, 0)== osOK) {
|
|
CAN_DJIMotor_Control(CAN_MOTOR_CHASSIS6020,&can_out,&can);
|
|
}
|
|
|
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
|
osDelayUntil(tick); /* 运行结束，等待下一个周期唤醒 */
|
|
}
|
|
}
|
|
|