95 lines
2.0 KiB
C
95 lines
2.0 KiB
C
|
/*
|
|||
|
|
上层控制任务
|
|||
|
|
*/
|
|||
|
|
/* Includes ----------------------------------------------------------------- */
|
|||
|
|
|
|||
|
|
#include "up.h"
|
|||
|
|
#include "user_task.h"
|
|||
|
|
#include "can_use.h"
|
|||
|
|
#include <cmsis_os2.h>
|
|||
|
|
|
|||
|
|
#include "vofa.h"
|
|||
|
|
|
|||
|
|
static CAN_t can;
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifdef DEBUG
|
|||
|
|
|
|||
|
|
CAN_Output_t UP_CAN_out;
|
|||
|
|
|
|||
|
|
UP_t UP ;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
#else
|
|||
|
|
|
|||
|
|
static CAN_Output_t up_can_out;
|
|||
|
|
|
|||
|
|
static UP_t UP;
|
|||
|
|
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**
|
|||
|
|
* \brief
|
|||
|
|
*
|
|||
|
|
* \param argument
|
|||
|
|
*/
|
|||
|
|
void Task_up(void *argument)
|
|||
|
|
{
|
|||
|
|
|
|||
|
|
(void)argument; /* 未使用argument，消除警告*/
|
|||
|
|
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_UP;
|
|||
|
|
|
|||
|
|
uint32_t tick = osKernelGetTickCount();
|
|||
|
|
up_init(&UP,&(task_runtime.config.chassis_config->up ),TASK_FREQ_UP);
|
|||
|
|
|
|||
|
|
while(1)
|
|||
|
|
{
|
|||
|
|
#ifdef DEBUG
|
|||
|
|
task_runtime.stack_water_mark.up = osThreadGetStackSpace(osThreadGetId());
|
|||
|
|
#endif
|
|||
|
|
UP_UpdateFeedback(&UP, &can) ;
|
|||
|
|
// GM6020_control(&UP, 100) ;
|
|||
|
|
// UP_M2006_angle(&UP, 180);
|
|||
|
|
// UP_M3508_speed(&UP, 500);
|
|||
|
|
// VESC_M5065_Control(&UP, 1000);
|
|||
|
|
ALL_Motor_Control(&UP,&UP_CAN_out);
|
|||
|
|
|
|||
|
|
/*imu数据获取*/
|
|||
|
|
osMessageQueueGet(task_runtime.msgq.imu.eulr, &UP.pos088.imu_eulr, NULL, 0);
|
|||
|
|
|
|||
|
|
osMessageQueueGet(task_runtime.msgq.imu.gyro, &UP.pos088.bmi088.gyro,NULL, 0);
|
|||
|
|
|
|||
|
|
osMessageQueueGet(task_runtime.msgq.imu.accl, &UP.pos088.bmi088.accl,NULL, 0);
|
|||
|
|
/*can上设备数据获取*/
|
|||
|
|
osMessageQueueGet(task_runtime.msgq.can.feedback.CAN_feedback, &can, NULL, 0);
|
|||
|
|
|
|||
|
|
|
|||
|
|
/*锁定RTOS（实时操作系统）内核，防止任务切换，直到 osKernelUnlock() 被调用*/
|
|||
|
|
osKernelLock();
|
|||
|
|
|
|||
|
|
/*解锁*/
|
|||
|
|
osKernelUnlock();
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &UP_CAN_out.motor3508, 0, 0); //发送数据
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.can.output.chassis6020 ,&UP_CAN_out.chassis6020 , 0, 0);
|
|||
|
|
osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0);
|
|||
|
|
|
|||
|
|
|
|||
|
|
vofa_send [0]=UP.vofa_send [0];
|
|||
|
|
vofa_send [1]=UP.vofa_send [1];
|
|||
|
|
vofa_tx_main (vofa_send);
|
|||
|
|
|
|||
|
|
tick += delay_tick;
|
|||
|
|
osDelayUntil(tick);
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|