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

imu完成

Browse Source
This commit is contained in:
Robofish 2025-10-05 03:25:18 +08:00
parent 4f4e485912
commit e4b1655698
7 changed files with 279 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -69,6 +69,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/module/config.c
# User/task sources
User/task/atti_esti.c
User/task/blink.c
User/task/ctrl_chassis.c
User/task/imu.c

162
User/task/atti_esti.c Normal file
View File

@ -0,0 +1,162 @@
/*
atti_esti Task
*/
/* Includes ----------------------------------------------------------------- */
#include "cmsis_os2.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/mm.h"
#include "bsp/pwm.h"
#include "bsp/gpio.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "task/user_task.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.00379243772f,0.00133061118f,-0.00154866849f},
};
static const KPID_Params_t imu_temp_ctrl_pid_param = {
.k = 0.2f,
.p = 1.0f,
.i = 0.01f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
};
/* 校准相关变量 */
typedef enum {
CALIB_IDLE, // 空闲状态
CALIB_RUNNING, // 正在校准
CALIB_DONE // 校准完成
} CalibState_t;
static CalibState_t calib_state = CALIB_IDLE;
static uint16_t calib_count = 0;
static struct {
float x_sum;
float y_sum;
float z_sum;
} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f};
static void start_gyro_calibration(void) {
if (calib_state == CALIB_IDLE) {
calib_state = CALIB_RUNNING;
calib_count = 0;
gyro_sum.x_sum = 0.0f;
gyro_sum.y_sum = 0.0f;
gyro_sum.z_sum = 0.0f;
}
}
#define CALIB_SAMPLES 5000 // 校准采样数量
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_atti_esti(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BMI088_Init(&bmi088,&cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM);
/* 注册按钮回调函数并启用中断 */
BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
/* 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);
BMI088_ParseGyro(&bmi088);
// IST8310_Parse(&ist8310);
/* 陀螺仪校准处理 */
if (calib_state == CALIB_RUNNING) {
/* 累加陀螺仪数据用于计算零偏 */
gyro_sum.x_sum += bmi088.gyro.x;
gyro_sum.y_sum += bmi088.gyro.y;
gyro_sum.z_sum += bmi088.gyro.z;
calib_count++;
/* 达到采样数量后计算平均值并更新零偏 */
if (calib_count >= CALIB_SAMPLES) {
/* 计算平均值作为零偏 */
cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES;
cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES;
cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES;
/* 校准完成，重置为空闲状态以便下次校准 */
calib_state = CALIB_IDLE;
/* 重新初始化BMI088以应用新的校准数据 */
BMI088_Init(&bmi088, &cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
}
}
/* 只有在非校准状态下才进行正常的姿态解析 */
if (calib_state != CALIB_RUNNING) {
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
}
osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.imu.accl);
osMessageQueueReset(task_runtime.msgq.imu.gyro);
osMessageQueueReset(task_runtime.msgq.imu.eulr);
osMessageQueueReset(task_runtime.msgq.imu.quat);
osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0);
osMessageQueuePut(task_runtime.msgq.imu.gyro, &bmi088.gyro, 0, 0);
osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_to_send, 0, 0);
osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0);
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f));
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

7
User/task/config.yaml
View File

@ -19,3 +19,10 @@
function: Task_ctrl_chassis
name: ctrl_chassis
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_atti_esti
name: atti_esti
stack: 256

193
User/task/imu.c
View File

@ -6,70 +6,30 @@
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/mm.h"
#include "bsp/pwm.h"
#include "bsp/gpio.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "task/user_task.h"
#include <string.h>
#include <stdint.h>
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* AHRS数据CAN ID定义 */
#define CAN_ID_AHRS_ACCL 0x301 /* 加速度计数据 */
#define CAN_ID_AHRS_GYRO 0x302 /* 陀螺仪数据 */
#define CAN_ID_AHRS_EULR 0x303 /* 欧拉角数据 */
#define CAN_ID_AHRS_QUAT 0x304 /* 四元数数据 */
/* 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},
};
static const KPID_Params_t imu_temp_ctrl_pid_param = {
.k = 0.2f,
.p = 1.0f,
.i = 0.01f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
};
/* 校准相关变量 */
typedef enum {
CALIB_IDLE, // 空闲状态
CALIB_RUNNING, // 正在校准
CALIB_DONE // 校准完成
} CalibState_t;
static CalibState_t calib_state = CALIB_IDLE;
static uint16_t calib_count = 0;
static struct {
float x_sum;
float y_sum;
float z_sum;
} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f};
#define CALIB_SAMPLES 5000 // 校准采样数量
AHRS_Accl_t accl;
AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr;
AHRS_Quaternion_t quat;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* 按钮回调函数：开始陀螺仪校准 */
static void start_gyro_calibration(void) {
if (calib_state == CALIB_IDLE) {
calib_state = CALIB_RUNNING;
calib_count = 0;
gyro_sum.x_sum = 0.0f;
gyro_sum.y_sum = 0.0f;
gyro_sum.z_sum = 0.0f;
}
}
/* Exported functions ------------------------------------------------------- */
void Task_imu(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
@ -82,73 +42,88 @@ void Task_imu(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BMI088_Init(&bmi088,&cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM);
/* 注册按钮回调函数并启用中断 */
BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration);
BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY);
BSP_CAN_Init();
/* 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);
BMI088_ParseGyro(&bmi088);
// IST8310_Parse(&ist8310);
/* 陀螺仪校准处理 */
if (calib_state == CALIB_RUNNING) {
/* 累加陀螺仪数据用于计算零偏 */
gyro_sum.x_sum += bmi088.gyro.x;
gyro_sum.y_sum += bmi088.gyro.y;
gyro_sum.z_sum += bmi088.gyro.z;
calib_count++;
/* 达到采样数量后计算平均值并更新零偏 */
if (calib_count >= CALIB_SAMPLES) {
/* 计算平均值作为零偏 */
cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES;
cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES;
cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES;
/* 获取加速度计数据并通过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个int16_t，每个2字节，共6字节 */
/* 校准完成，重置为空闲状态以便下次校准 */
calib_state = CALIB_IDLE;
/* 将float转换为int16_t (乘以100，加速度一般在±20g范围内) */
int16_t x_int = (int16_t)(accl.x * 100);
int16_t y_int = (int16_t)(accl.y * 100);
int16_t z_int = (int16_t)(accl.z * 100);
/* 重新初始化BMI088以应用新的校准数据 */
BMI088_Init(&bmi088, &cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
}
}
/* 只有在非校准状态下才进行正常的姿态解析 */
if (calib_state != CALIB_RUNNING) {
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
memcpy(&accl_frame.data[0], &x_int, sizeof(int16_t));
memcpy(&accl_frame.data[2], &y_int, sizeof(int16_t));
memcpy(&accl_frame.data[4], &z_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
}
osKernelUnlock();
BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f));
/* 获取陀螺仪数据并通过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个int16_t，每个2字节，共6字节 */
/* 将float转换为int16_t (乘以10，陀螺仪一般在±2000°/s范围内) */
int16_t x_int = (int16_t)(gyro.x * 10);
int16_t y_int = (int16_t)(gyro.y * 10);
int16_t z_int = (int16_t)(gyro.z * 10);
memcpy(&gyro_frame.data[0], &x_int, sizeof(int16_t));
memcpy(&gyro_frame.data[2], &y_int, sizeof(int16_t));
memcpy(&gyro_frame.data[4], &z_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame);
}
/* 获取欧拉角数据并通过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个int16_t，每个2字节，共6字节 */
/* 将角度转换为int16_t (乘以100，角度范围-180~180°，精度0.01°) */
int16_t yaw_int = (int16_t)(eulr.yaw * 100);
int16_t pit_int = (int16_t)(eulr.pit * 100);
int16_t rol_int = (int16_t)(eulr.rol * 100);
memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t));
memcpy(&eulr_frame.data[2], &pit_int, sizeof(int16_t));
memcpy(&eulr_frame.data[4], &rol_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame);
}
/* 获取四元数数据并通过CAN发送 - 压缩为16位整数 */
if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t quat_frame;
quat_frame.id = CAN_ID_AHRS_QUAT;
quat_frame.dlc = 8; /* 4个int16_t，每个2字节，共8字节 */
/* 将四元数转换为int16_t (乘以32000，四元数范围-1~1，充分利用int16_t范围) */
int16_t q0_int = (int16_t)(quat.q0 * 32000);
int16_t q1_int = (int16_t)(quat.q1 * 32000);
int16_t q2_int = (int16_t)(quat.q2 * 32000);
int16_t q3_int = (int16_t)(quat.q3 * 32000);
memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t));
memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t));
memcpy(&quat_frame.data[4], &q2_int, sizeof(int16_t));
memcpy(&quat_frame.data[6], &q3_int, sizeof(int16_t));
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}
}

7
User/task/init.c
View File

@ -7,7 +7,7 @@
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "component/ahrs.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -33,10 +33,15 @@ void Task_Init(void *argument) {
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu);
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
// 创建消息队列
/* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.imu.accl = osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
task_runtime.msgq.imu.gyro = osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
task_runtime.msgq.imu.eulr = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
task_runtime.msgq.imu.quat = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

5
User/task/user_task.c
View File

@ -23,4 +23,9 @@ const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_atti_esti = {
.name = "atti_esti",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};

14
User/task/user_task.h
View File

@ -16,12 +16,14 @@ extern "C" {
#define BLINK_FREQ (500.0)
#define IMU_FREQ (500.0)
#define CTRL_CHASSIS_FREQ (500.0)
#define ATTI_ESTI_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
#define BLINK_INIT_DELAY (0)
#define IMU_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (0)
#define ATTI_ESTI_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -34,11 +36,18 @@ typedef struct {
osThreadId_t blink;
osThreadId_t imu;
osThreadId_t ctrl_chassis;
osThreadId_t atti_esti;
} thread;
/* USER MESSAGE BEGIN */
struct {
osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
struct {
osMessageQueueId_t accl;
osMessageQueueId_t gyro;
osMessageQueueId_t eulr;
osMessageQueueId_t quat;
}imu;
} msgq;
/* USER MESSAGE END */
@ -58,6 +67,7 @@ typedef struct {
UBaseType_t blink;
UBaseType_t imu;
UBaseType_t ctrl_chassis;
UBaseType_t atti_esti;
} stack_water_mark;
/* 各任务运行频率 */
@ -65,6 +75,7 @@ typedef struct {
float blink;
float imu;
float ctrl_chassis;
float atti_esti;
} freq;
/* 任务最近运行时间 */
@ -72,6 +83,7 @@ typedef struct {
float blink;
float imu;
float ctrl_chassis;
float atti_esti;
} last_up_time;
} Task_Runtime_t;
@ -84,12 +96,14 @@ extern const osThreadAttr_t attr_init;
extern const osThreadAttr_t attr_blink;
extern const osThreadAttr_t attr_imu;
extern const osThreadAttr_t attr_ctrl_chassis;
extern const osThreadAttr_t attr_atti_esti;
/* 任务函数声明 */
void Task_Init(void *argument);
void Task_blink(void *argument);
void Task_imu(void *argument);
void Task_ctrl_chassis(void *argument);
void Task_atti_esti(void *argument);
#ifdef __cplusplus
}