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

合并 有可能疯的底盘 到 MC_02

Browse Source
This commit is contained in:
Robofish 2026-02-25 15:43:42 +08:00
commit f84458f243
10 changed files with 447 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Src/can.c
View File

@ -66,7 +66,7 @@ void MX_CAN1_Init(void)
hcan1.Init.AutoWakeUp = DISABLE;
hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = ENABLE;
hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
{
Error_Handler();
@ -98,7 +98,7 @@ void MX_CAN2_Init(void)
hcan2.Init.AutoWakeUp = DISABLE;
hcan2.Init.AutoRetransmission = ENABLE;
hcan2.Init.ReceiveFifoLocked = DISABLE;
hcan2.Init.TransmitFifoPriority = ENABLE;
hcan2.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan2) != HAL_OK)
{
Error_Handler();

11
DevC.ioc
View File

@ -15,16 +15,15 @@ ADC3.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES
CAD.formats=
CAD.pinconfig=
CAD.provider=
CAN1.ABOM=DISABLE
CAN1.BS1=CAN_BS1_6TQ
CAN1.BS2=CAN_BS2_7TQ
CAN1.CalculateBaudRate=1000000
CAN1.CalculateTimeBit=1000
CAN1.CalculateTimeQuantum=71.42857142857143
CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART
CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2,NART,TXFP
CAN1.NART=ENABLE
CAN1.Prescaler=3
CAN1.TXFP=ENABLE
CAN1.TXFP=DISABLE
CAN2.BS1=CAN_BS1_6TQ
CAN2.BS2=CAN_BS2_7TQ
CAN2.CalculateBaudRate=1000000
@ -33,7 +32,7 @@ CAN2.CalculateTimeQuantum=71.42857142857143
CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART
CAN2.NART=ENABLE
CAN2.Prescaler=3
CAN2.TXFP=ENABLE
CAN2.TXFP=DISABLE
Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH
Dma.I2C2_TX.2.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.I2C2_TX.2.Instance=DMA1_Stream7
@ -285,7 +284,7 @@ NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
MxCube.Version=6.2.0
MxDb.Version=DB.6.0.20
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
@ -608,7 +607,7 @@ ProjectManager.ProjectFileName=DevC.ioc
ProjectManager.ProjectName=DevC
ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x1000
ProjectManager.StackSize=0x2000
ProjectManager.TargetToolchain=CMake
ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=

2
STM32F407XX_FLASH.ld
View File

@ -75,7 +75,7 @@ FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K
_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */
/* Generate a link error if heap and stack don't fit into RAM */
_Min_Heap_Size = 0x1000; /* required amount of heap */
_Min_Stack_Size = 0x1000; /* required amount of stack */
_Min_Stack_Size = 0x2000; /* required amount of stack */
>>>>>>> upper
/* Define output sections */

271
User/device/dm_imu.c Normal file
View File

@ -0,0 +1,271 @@
/*
DM_IMU数据获取（CAN）
*/
/* Includes ----------------------------------------------------------------- */
#include "dm_imu.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include <string.h>
/* Private define ----------------------------------------------------------- */
#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms
#define ACCEL_CAN_MAX (58.8f)
#define ACCEL_CAN_MIN (-58.8f)
#define GYRO_CAN_MAX (34.88f)
#define GYRO_CAN_MIN (-34.88f)
#define PITCH_CAN_MAX (90.0f)
#define PITCH_CAN_MIN (-90.0f)
#define ROLL_CAN_MAX (180.0f)
#define ROLL_CAN_MIN (-180.0f)
#define YAW_CAN_MAX (180.0f)
#define YAW_CAN_MIN (-180.0f)
#define TEMP_MIN (0.0f)
#define TEMP_MAX (60.0f)
#define Quaternion_MIN (-1.0f)
#define Quaternion_MAX (1.0f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
/* Private function --------------------------------------------------------- */
static uint8_t count = 0; // 计数器，用于判断设备是否离线
/**
* @brief: 无符号整数转换为浮点数函数
*/
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/**
* @brief 解析加速度计数据
*/
static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int8_t temp = data[1];
uint16_t acc_x_raw = (data[3] << 8) | data[2];
uint16_t acc_y_raw = (data[5] << 8) | data[4];
uint16_t acc_z_raw = (data[7] << 8) | data[6];
imu->data.temp = (float)temp;
imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
return DEVICE_OK;
}
/**
* @brief 解析陀螺仪数据
*/
static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
uint16_t gyro_x_raw = (data[3] << 8) | data[2];
uint16_t gyro_y_raw = (data[5] << 8) | data[4];
uint16_t gyro_z_raw = (data[7] << 8) | data[6];
imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
return DEVICE_OK;
}
/**
* @brief 解析欧拉角数据
*/
static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
uint16_t pit_raw = (data[3] << 8) | data[2];
uint16_t yaw_raw = (data[5] << 8) | data[4];
uint16_t rol_raw = (data[7] << 8) | data[6];
imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
return DEVICE_OK;
}
/**
* @brief 解析四元数数据
*/
static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
int z = ((data[6] & 0x3F) << 8) | data[7];
imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化DM IMU设备
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
if (imu == NULL || param == NULL) {
return DEVICE_ERR_NULL;
}
// 初始化设备头部
imu->header.online = false;
imu->header.last_online_time = 0;
// 配置参数
imu->param.can = param->can;
imu->param.can_id = param->can_id;
imu->param.device_id = param->device_id;
imu->param.master_id = param->master_id;
// 清零数据
memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
// 注册CAN接收队列，用于接收回复报文
int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
if (result != BSP_OK) {
return DEVICE_ERR;
}
return DEVICE_OK;
}
/**
* @brief 请求IMU数据
*/
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
// 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
uint8_t tx_data[4] = {
imu->param.device_id & 0xFF, // id_L
(imu->param.device_id >> 8) & 0xFF, // id_H
(uint8_t)rid, // RID
0xCC // 固定值
};
// 发送标准数据帧
BSP_CAN_StdDataFrame_t frame = {
.id = imu->param.can_id,
.dlc = 4,
};
memcpy(frame.data, tx_data, 4);
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
*/
int8_t DM_IMU_Update(DM_IMU_t *imu) {
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
BSP_CAN_Message_t msg;
int8_t result;
bool data_received = false;
// 持续接收所有可用消息
while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
// 验证回复数据格式（至少检查数据长度）
if (msg.dlc < 3) {
continue; // 跳过无效消息
}
// 根据数据位的第0位确定反馈报文类型
uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
// 根据RID类型解析数据
int8_t parse_result = DEVICE_ERR;
switch (rid) {
case 0x01: // RID_ACCL
parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
break;
case 0x02: // RID_GYRO
parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
break;
case 0x03: // RID_EULER
parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
break;
case 0x04: // RID_QUATERNION
parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
break;
default:
continue; // 跳过未知类型的消息
}
// 如果解析成功，标记为收到数据
if (parse_result == DEVICE_OK) {
data_received = true;
}
}
// 如果收到任何有效数据，更新设备状态
if (data_received) {
imu->header.online = true;
imu->header.last_online_time = BSP_TIME_Get_ms();
return DEVICE_OK;
}
return DEVICE_ERR;
}
/**
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
*/
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
if (imu == NULL) {
return DEVICE_ERR_NULL;
}
switch (count) {
case 0:
DM_IMU_Request(imu, RID_ACCL);
break;
case 1:
DM_IMU_Request(imu, RID_GYRO);
break;
case 2:
DM_IMU_Request(imu, RID_EULER);
break;
case 3:
DM_IMU_Request(imu, RID_QUATERNION);
DM_IMU_Update(imu); // 更新所有数据
break;
}
count++;
if (count >= 4) {
count = 0; // 重置计数器
return DEVICE_OK;
}
return DEVICE_ERR;
}
/**
* @brief 检查设备是否在线
*/
bool DM_IMU_IsOnline(DM_IMU_t *imu) {
if (imu == NULL) {
return false;
}
uint32_t current_time = BSP_TIME_Get_ms();
return imu->header.online &&
(current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
}

90
User/device/dm_imu.h Normal file
View File

@ -0,0 +1,90 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "component/ahrs.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define DM_IMU_CAN_ID_DEFAULT 0x6FF
#define DM_IMU_ID_DEFAULT 0x42
#define DM_IMU_MST_ID_DEFAULT 0x43
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
BSP_CAN_t can; // CAN总线句柄
uint16_t can_id; // CAN通信ID
uint8_t device_id; // 设备ID
uint8_t master_id; // 主机ID
} DM_IMU_Param_t;
typedef enum {
RID_ACCL = 0x01, // 加速度计
RID_GYRO = 0x02, // 陀螺仪
RID_EULER = 0x03, // 欧拉角
RID_QUATERNION = 0x04, // 四元数
} DM_IMU_RID_t;
typedef struct {
AHRS_Accl_t accl; // 加速度计
AHRS_Gyro_t gyro; // 陀螺仪
AHRS_Eulr_t euler; // 欧拉角
AHRS_Quaternion_t quat; // 四元数
float temp; // 温度
} DM_IMU_Data_t;
typedef struct {
DEVICE_Header_t header;
DM_IMU_Param_t param; // IMU参数配置
DM_IMU_Data_t data; // IMU数据
} DM_IMU_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化DM IMU设备
* @param imu DM IMU设备结构体指针
* @param param IMU参数配置指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
/**
* @brief 请求IMU数据
* @param imu DM IMU设备结构体指针
* @param rid 请求的数据类型
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
/**
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
* @param imu DM IMU设备结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_Update(DM_IMU_t *imu);
/**
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
* @param imu DM IMU设备结构体指针
* @return DEVICE_OK 成功，其他值失败
*/
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
/**
* @brief 检查设备是否在线
* @param imu DM IMU设备结构体指针
* @return true 在线，false 离线
*/
bool DM_IMU_IsOnline(DM_IMU_t *imu);
#ifdef __cplusplus
}
#endif

95
User/task/1.c
View File

@ -1,95 +0,0 @@
/*
imu Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/pwm.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
BMI088_t bmi088;
AHRS_t gimbal_ahrs;
AHRS_Magn_t magn;
AHRS_Eulr_t eulr_to_send;
KPID_t imu_temp_ctrl_pid;
BMI088_Cali_t cali_bmi088= {
.gyro_offset = {0.0f,0.0f,0.0f},
};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_imu(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BMI088_Init(&bmi088,&cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
BMI088_WaitNew();
BMI088_AcclStartDmaRecv();
BMI088_AcclWaitDmaCplt();
BMI088_GyroStartDmaRecv();
BMI088_GyroWaitDmaCplt();
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
osKernelLock();
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
BMI088_ParseAccl(&bmi088);
/* 扩大加速度数据10倍，并交换x和y */
float temp_x = bmi088.accl.x * 10.0f;
float temp_y = bmi088.accl.y * 10.0f;
bmi088.accl.x = temp_y;
bmi088.accl.y = -temp_x;
bmi088.accl.z *= 10.0f;
BMI088_ParseGyro(&bmi088);
/* 交换陀螺仪x和y */
float temp_gyro_x = bmi088.gyro.x;
bmi088.gyro.x = bmi088.gyro.y;
bmi088.gyro.y = -temp_gyro_x;
// IST8310_Parse(&ist8310);
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
/* 交换pit和rol */
float temp_rol = eulr_to_send.rol;
eulr_to_send.rol = eulr_to_send.pit;
eulr_to_send.pit = temp_rol;
osKernelUnlock();
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

1
User/task/atti_esti.c
View File

@ -4,7 +4,6 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/pwm.h"

7
User/task/config.yaml
View File

@ -83,3 +83,10 @@
function: Task_debug
name: debug
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_monitor
name: monitor
stack: 256

81
User/task/imu.c
View File

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

5
User/task/user_task.c
View File

@ -66,4 +66,9 @@ const osThreadAttr_t attr_debug = {
.name = "debug",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_monitor = {
.name = "monitor",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};