R2_UP/User/task/up_task.c
2025-03-13 19:07:44 +08:00

98 lines
2.1 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*
上层控制任务
*/
/* 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, 00);
ALL_Motor_Control(&UP,&UP_CAN_out);
// GO_M8010_send_data(&huart6, 0,0,0,5,10,0.12,0.08);
osDelay(1);
/*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);
}
}