robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

加imu铝箔

Browse Source
This commit is contained in:
Robofish 2025-10-19 13:37:10 +08:00
parent 3149179e9c
commit 63db275f58
1 changed files with 66 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
User/task/imu.c
View File

@ -10,6 +10,7 @@
#include "bsp/can.h" #include "bsp/can.h"
#include "bsp/time.h" #include "bsp/time.h"
#include "component/ahrs.h" #include "component/ahrs.h"
#include "component/filter.h"
#include <string.h> #include <string.h>
#include <stdint.h> #include <stdint.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
@ -50,6 +51,25 @@ AHRS_Accl_t accl;
AHRS_Gyro_t gyro; AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr; AHRS_Eulr_t eulr;
AHRS_Quaternion_t quat; AHRS_Quaternion_t quat;
/* 低通滤波器定义 */
/* 加速度计3轴滤波器 */
LowPassFilter2p_t accl_filter_x;
LowPassFilter2p_t accl_filter_y;
LowPassFilter2p_t accl_filter_z;
/* 陀螺仪3轴滤波器 */
LowPassFilter2p_t gyro_filter_x;
LowPassFilter2p_t gyro_filter_y;
LowPassFilter2p_t gyro_filter_z;
/* 欧拉角3轴滤波器 */
LowPassFilter2p_t eulr_filter_yaw;
LowPassFilter2p_t eulr_filter_pitch;
LowPassFilter2p_t eulr_filter_roll;
/* 滤波器参数定义 */
#define FILTER_CUTOFF_FREQ (30.0f) /* 截止频率50Hz */
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -66,21 +86,42 @@ void Task_imu(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
BSP_CAN_Init(); BSP_CAN_Init();
/* 初始化低通滤波器 */
/* 加速度计滤波器初始化 */
LowPassFilter2p_Init(&accl_filter_x, IMU_FREQ, FILTER_CUTOFF_FREQ);
LowPassFilter2p_Init(&accl_filter_y, IMU_FREQ, FILTER_CUTOFF_FREQ);
LowPassFilter2p_Init(&accl_filter_z, IMU_FREQ, FILTER_CUTOFF_FREQ);
/* 陀螺仪滤波器初始化 */
LowPassFilter2p_Init(&gyro_filter_x, IMU_FREQ, FILTER_CUTOFF_FREQ);
LowPassFilter2p_Init(&gyro_filter_y, IMU_FREQ, FILTER_CUTOFF_FREQ);
LowPassFilter2p_Init(&gyro_filter_z, IMU_FREQ, FILTER_CUTOFF_FREQ);
/* 欧拉角滤波器初始化 */
LowPassFilter2p_Init(&eulr_filter_yaw, IMU_FREQ, FILTER_CUTOFF_FREQ);
LowPassFilter2p_Init(&eulr_filter_pitch, IMU_FREQ, FILTER_CUTOFF_FREQ);
LowPassFilter2p_Init(&eulr_filter_roll, IMU_FREQ, FILTER_CUTOFF_FREQ);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
/* 获取加速度计数据并通过CAN发送 - 每轴使用16位编码 */ /* 获取加速度计数据，应用滤波器后通过CAN发送 - 每轴使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) { if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t accl_frame; BSP_CAN_StdDataFrame_t accl_frame;
accl_frame.id = CAN_ID_AHRS_ACCL; accl_frame.id = CAN_ID_AHRS_ACCL;
accl_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */ accl_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */
/* 限制并映射加速度数据到16位整数 */ /* 对加速度数据应用低通滤波器 */
float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX); float ax_filtered = LowPassFilter2p_Apply(&accl_filter_x, accl.x);
float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX); float ay_filtered = LowPassFilter2p_Apply(&accl_filter_y, accl.y);
float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX); float az_filtered = LowPassFilter2p_Apply(&accl_filter_z, accl.z);
/* 限制并映射滤波后的加速度数据到16位整数 */
float ax = CLAMP(ax_filtered, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
float ay = CLAMP(ay_filtered, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
float az = CLAMP(az_filtered, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX); int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX); int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
@ -93,16 +134,21 @@ void Task_imu(void *argument) {
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
} }
/* 获取陀螺仪数据并通过CAN发送 - 每轴使用16位编码 */ /* 获取陀螺仪数据，应用滤波器后通过CAN发送 - 每轴使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) { if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t gyro_frame; BSP_CAN_StdDataFrame_t gyro_frame;
gyro_frame.id = CAN_ID_AHRS_GYRO; gyro_frame.id = CAN_ID_AHRS_GYRO;
gyro_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */ gyro_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */
/* 限制并映射陀螺仪数据到16位整数 */ /* 对陀螺仪数据应用低通滤波器 */
float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX); float gx_filtered = LowPassFilter2p_Apply(&gyro_filter_x, gyro.x);
float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX); float gy_filtered = LowPassFilter2p_Apply(&gyro_filter_y, gyro.y);
float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX); float gz_filtered = LowPassFilter2p_Apply(&gyro_filter_z, gyro.z);
/* 限制并映射滤波后的陀螺仪数据到16位整数 */
float gx = CLAMP(gx_filtered, GYRO_CAN_MIN, GYRO_CAN_MAX);
float gy = CLAMP(gy_filtered, GYRO_CAN_MIN, GYRO_CAN_MAX);
float gz = CLAMP(gz_filtered, GYRO_CAN_MIN, GYRO_CAN_MAX);
int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX); int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX);
int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX); int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX);
@ -115,16 +161,21 @@ void Task_imu(void *argument) {
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame);
} }
/* 获取欧拉角数据并通过CAN发送 - 每角使用16位编码 */ /* 获取欧拉角数据，应用滤波器后通过CAN发送 - 每角使用16位编码 */
if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) { if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t eulr_frame; BSP_CAN_StdDataFrame_t eulr_frame;
eulr_frame.id = CAN_ID_AHRS_EULR; eulr_frame.id = CAN_ID_AHRS_EULR;
eulr_frame.dlc = 6; /* 3个角度 × 2字节 = 6字节 */ eulr_frame.dlc = 6; /* 3个角度 × 2字节 = 6字节 */
/* 限制并映射欧拉角数据到16位整数 */ /* 对欧拉角数据应用低通滤波器 */
float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX); float yaw_filtered = LowPassFilter2p_Apply(&eulr_filter_yaw, eulr.yaw);
float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX); float pitch_filtered = LowPassFilter2p_Apply(&eulr_filter_pitch, eulr.pit);
float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX); float roll_filtered = LowPassFilter2p_Apply(&eulr_filter_roll, eulr.rol);
/* 限制并映射滤波后的欧拉角数据到16位整数 */
float yaw = CLAMP(yaw_filtered, YAW_CAN_MIN, YAW_CAN_MAX);
float pitch = CLAMP(pitch_filtered, PITCH_CAN_MIN, PITCH_CAN_MAX);
float roll = CLAMP(roll_filtered, ROLL_CAN_MIN, ROLL_CAN_MAX);
int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX); int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX);
int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX); int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX);