108 lines
2.8 KiB
C
108 lines
2.8 KiB
C
/*
|
||
底盘控制任务〄1<E38084>7
|
||
|
||
*/
|
||
/* Includes ----------------------------------------------------------------- */
|
||
|
||
#include "Chassis.h"
|
||
#include "user_task.h"
|
||
#include "vofa.h"
|
||
|
||
static CAN_t can;
|
||
|
||
#ifdef DEBUG
|
||
|
||
CAN_Output_t out;
|
||
|
||
Chassis_t chassis;
|
||
|
||
CMD_t ctrl;
|
||
|
||
Action_POS_t action_pos;
|
||
#else
|
||
|
||
static CAN_Output_t out;
|
||
|
||
static Chassis_t chassis;
|
||
|
||
static Chassis_Ctrl_t ctrl;
|
||
|
||
static Action_POS_t action_pos;
|
||
#endif
|
||
|
||
/**
|
||
* \brief 底盘任务
|
||
*
|
||
* \param argument 未使甄1<E79484>7
|
||
*/
|
||
void Task_Chassis(void *argument)
|
||
{
|
||
|
||
(void)argument; /* 未使用argument，消除警告*/
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CHASSIS;
|
||
|
||
Chassis_init(&chassis,&(task_runtime.config.chassis_config->chassis),TASK_FREQ_CHASSIS);
|
||
|
||
uint32_t tick = osKernelGetTickCount();
|
||
|
||
while(1)
|
||
{
|
||
#ifdef DEBUG
|
||
task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
|
||
#endif
|
||
//暂时没发现不同任务同时获取imu数据会出现问题
|
||
|
||
osMessageQueueGet(task_runtime.msgq.imu.eulr, &chassis.pos088.imu_eulr, NULL, 0);
|
||
|
||
osMessageQueueGet(task_runtime.msgq.imu.gyro, &chassis.pos088.bmi088.gyro,NULL, 0);
|
||
|
||
osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed6020, &can, NULL, 0);
|
||
|
||
osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed5065, &can, NULL, 0);
|
||
|
||
// osMessageQueueGet(task_runtime.msgq.can.feedback.pitchfed6020, &can, NULL, 0);
|
||
|
||
// osMessageQueueGet(task_runtime.msgq.can.feedback.chassisfed3508, &can, NULL, 0);
|
||
|
||
osMessageQueueGet(task_runtime.msgq.cmd.chassis,&ctrl, NULL, 0);
|
||
|
||
osMessageQueueGet(task_runtime.msgq.action.Chassis,&action_pos,NULL,0);
|
||
|
||
osKernelLock();
|
||
|
||
Chassis_UpdateFeedback(&chassis, &can);
|
||
|
||
Chassis_Control(&chassis,&ctrl,&out, &action_pos);
|
||
|
||
osKernelUnlock();
|
||
|
||
osMessageQueueReset(task_runtime.msgq.can.output.chassis6020);
|
||
osMessageQueuePut(task_runtime.msgq.can.output.chassis6020, &out.chassis6020, 0, 0);
|
||
|
||
osMessageQueueReset(task_runtime.msgq.can.output.chassis5065);
|
||
osMessageQueuePut(task_runtime.msgq.can.output.chassis5065, &out.chassis5065, 0, 0);
|
||
|
||
// osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);
|
||
// osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor3508, 0, 0);
|
||
|
||
// osMessageQueueReset(task_runtime.msgq.can.output.pitch6020);
|
||
// osMessageQueuePut(task_runtime.msgq.can.output.pitch6020, &out.pitch6020, 0, 0);
|
||
|
||
// vofa_send[0] = chassis.vofa_send[0];
|
||
// vofa_send[1] = chassis.vofa_send[1];
|
||
// vofa_send[2] = chassis.vofa_send[2];
|
||
// vofa_send[3] = chassis.vofa_send[3];
|
||
// vofa_send[4] = chassis.vofa_send[4];
|
||
// vofa_send[5] = chassis.vofa_send[5];
|
||
// vofa_send[6] = chassis.vofa_send[6];
|
||
// vofa_send[7] = chassis.vofa_send[7];
|
||
//
|
||
// vofa_tx_main(vofa_send);
|
||
|
||
tick += delay_tick;
|
||
osDelayUntil(tick);
|
||
|
||
}
|
||
|
||
}
|