84 lines
3.0 KiB
C
84 lines
3.0 KiB
C
/*
|
||
vofa Task
|
||
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "task/user_task.h"
|
||
/* USER INCLUDE BEGIN */
|
||
#include "module/balance_chassis.h"
|
||
#include "device/vofa.h"
|
||
/* USER INCLUDE END */
|
||
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* USER STRUCT BEGIN */
|
||
Chassis_t chassis_vofa;
|
||
/* USER STRUCT END */
|
||
|
||
/* Private function --------------------------------------------------------- */
|
||
/* Exported functions ------------------------------------------------------- */
|
||
void Task_vofa(void *argument) {
|
||
(void)argument; /* 未使用argument，消除警告 */
|
||
|
||
|
||
/* 计算任务运行到指定频率需要等待的tick数 */
|
||
const uint32_t delay_tick = osKernelGetTickFreq() / VOFA_FREQ;
|
||
|
||
osDelay(VOFA_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||
|
||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||
/* USER CODE INIT BEGIN */
|
||
VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
|
||
/* USER CODE INIT END */
|
||
|
||
while (1) {
|
||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||
/* USER CODE BEGIN */
|
||
// 从消息队列获取chassis数据
|
||
if (osMessageQueueGet(task_runtime.msgq.chassis.vofa, &chassis_vofa, NULL, 0) == osOK) {
|
||
// 准备要发送的数据通道
|
||
float channels[16];
|
||
|
||
// 通道0-1: 左右腿的VMC腿长
|
||
channels[0] = chassis_vofa.vmc_[0].leg.L0;
|
||
channels[1] = chassis_vofa.vmc_[1].leg.L0;
|
||
|
||
// 通道2-3: 左右腿的VMC角度
|
||
channels[2] = chassis_vofa.vmc_[0].leg.theta;
|
||
channels[3] = chassis_vofa.vmc_[1].leg.theta;
|
||
|
||
// 通道4-6: IMU欧拉角 (pitch, roll, yaw)
|
||
channels[4] = chassis_vofa.feedback.imu.euler.pit;
|
||
channels[5] = chassis_vofa.feedback.imu.euler.rol;
|
||
channels[6] = chassis_vofa.feedback.imu.euler.yaw;
|
||
|
||
// 通道7-8: 左右轮速度
|
||
channels[7] = chassis_vofa.vmc_[0].leg.d_theta;
|
||
channels[8] = chassis_vofa.vmc_[0].leg.dd_theta;
|
||
|
||
// 通道9-10: 机体位置和速度
|
||
channels[9] = chassis_vofa.chassis_state.position_x;
|
||
channels[10] = chassis_vofa.chassis_state.velocity_x;
|
||
|
||
// 通道11-12: 左右腿的虚拟支撑力
|
||
channels[11] = chassis_vofa.vmc_[0].leg.F_virtual;
|
||
channels[12] = chassis_vofa.vmc_[1].leg.F_virtual;
|
||
|
||
// 通道13-14: 左右腿的虚拟摆力矩
|
||
channels[13] = chassis_vofa.vmc_[0].leg.T_virtual;
|
||
channels[14] = chassis_vofa.vmc_[1].leg.T_virtual;
|
||
|
||
// 通道15: yaw控制力矩
|
||
channels[15] = chassis_vofa.yaw_control.yaw_force;
|
||
|
||
// 使用JustFloat协议和DMA发送
|
||
VOFA_Send(channels, 16, true);
|
||
}
|
||
/* USER CODE END */
|
||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||
}
|
||
|
||
} |