pima/Trinity-DevPCB-V1.0
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

- Fixed IMU calibration function bugs

Browse Source 
- Fixed IMU data processing bugs
- Fixed madgwick init bug
- Inverted magn axis relative to IMU (180 degree rotation) in madgwick
- Acc bias currently set to 0 and 1 for gravity
- Gyro values now rad/s instead of deg/s as madgwick requires
This commit is contained in:
pima committed 2026-10-04 18:03:02 +02:00
1 parent 1409bad40b
commit 570f0c12f9
19 files changed
+45215 -45317
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

Core/Src/main.c
+32 -23
View File
@@ -31,6 +31,7 @@
#include "bmm350_defs.h"
#include "bmm350_oor.h"
#include "task.h"
#include "trinity_helperlib.h"
// CDC functionality
#include "usbd_cdc_if.h"
@@ -510,7 +511,7 @@ static void MX_SPI1_Init(void)
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
@@ -741,18 +742,33 @@ void Start_DataAnalysis(void *argument)
Quat_Dat.SensorAddress = BMM350_I2C_ADSEL_SET_LOW;
Quat_Dat.EndByte = PACKET_END_BYTE;
MadgwickAHRS_init(&IMUQuat, 6400.0f, 0.00001f);
MadgwickAHRS_init(&IMUQuat, 0.1f, 400.0f);
/* Infinite loop */
for(;;)
{
// TODO: Sample the IMU as a free-low pass to match the magnetometers
// ODR
// TODO: Add changing parameters from Trinity Control
// Don't feed Madgwick at 6.4 kHz. A 9-DOF update at that rate is expensive
// on an STM32WB, and a USB packet per sample can't keep up anyway. Average
// 16 samples in the IMU task to get 400 Hz, which also acts as a free low-pass
// filter. Let the IMU be the fusion clock and read the mag non-blocking.
//
osMessageQueueGet(Q_BMMHandle, &magn_data, NULL, osWaitForever);
osMessageQueueGet(Q_ICMHandle, &imu_data, NULL, osWaitForever);
// MadgwickAHRS_update(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
// imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2], // ax, ay, yz
// magn_data.x, magn_data.y, magn_data.z); // mx, my, mz
// This code has the magnetometer axis rotated by 180 degrees relative to the IMU
// because the axis are different. IMU xy-coordinate system and magnetometer yx-coordinate system
MadgwickAHRS_update(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2], // ax, ay, yz
magn_data.x, magn_data.y, magn_data.z); // mx, my, mz
magn_data.y, magn_data.x, -magn_data.z); // mx, my, mz
// MadgwickAHRS_update_IMU(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
// imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2]); // ax, ay, yz)
@@ -808,9 +824,8 @@ void Start_DataAnalysis(void *argument)
void Start_ICM45686(void *argument)
{
/* USER CODE BEGIN Start_ICM45686 */
HAL_StatusTypeDef status;
// char output_buffer[128];
xIMU_Task = osThreadGetId();
HAL_StatusTypeDef status;
uint8_t raw_imu_data[15];
int16_t imu_sensor_data[7];
ICM45686_Data processed_data;
@@ -823,7 +838,7 @@ void Start_ICM45686(void *argument)
imu.acc_ssf = ICM45686_ACC_SSF[ICM45686_ACC_FS_16G >> 4];
imu.gyro_fs = ICM45686_GYRO_FS_2000DPS;
imu.gyro_ssf = ICM45686_GYRO_SSF[ICM45686_GYRO_FS_2000DPS >> 4];
imu.odr = ICM45686_ODR_6_4kHz_LN ;
imu.odr = ICM45686_ODR_400Hz_LNLP ;
uint8_t tx[15] = {0x00};
tx[0] = 0x00 | 0x80;
@@ -834,7 +849,7 @@ void Start_ICM45686(void *argument)
configure_and_enable_int_icm(&imu);
ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, 0);
ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, 0);
calibrate_icm(&imu, 25000);
calibrate_icm(&imu, 1000);
/* Infinite loop */
for(;;)
{
@@ -852,13 +867,13 @@ void Start_ICM45686(void *argument)
imu_sensor_data[5] = (int16_t)(raw_imu_data[11] << 8 | raw_imu_data[12]); // g_z
imu_sensor_data[6] = (int16_t)(raw_imu_data[13] << 8 | raw_imu_data[14]); // t
processed_data.processed_imu_data[0] = (float)imu_sensor_data[0] / (float)imu.acc_fs;
processed_data.processed_imu_data[1] = (float)imu_sensor_data[1] / (float)imu.acc_fs;
processed_data.processed_imu_data[2] = (float)imu_sensor_data[2] / (float)imu.acc_fs;
processed_data.processed_imu_data[3] = (float)imu_sensor_data[3] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[4] = (float)imu_sensor_data[4] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[5] = (float)imu_sensor_data[5] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[6] = (float)(imu_sensor_data[6] / 128.0 + 25.0);
processed_data.processed_imu_data[0] = (float)imu_sensor_data[0] / imu.acc_ssf;
processed_data.processed_imu_data[1] = (float)imu_sensor_data[1] / imu.acc_ssf;
processed_data.processed_imu_data[2] = (float)imu_sensor_data[2] / imu.acc_ssf;
processed_data.processed_imu_data[3] = (float)imu_sensor_data[3] / imu.gyro_ssf * DEG2RAD;;
processed_data.processed_imu_data[4] = (float)imu_sensor_data[4] / imu.gyro_ssf * DEG2RAD;;
processed_data.processed_imu_data[5] = (float)imu_sensor_data[5] / imu.gyro_ssf * DEG2RAD;;
processed_data.processed_imu_data[6] = (float)(imu_sensor_data[6] / 128.0f + 25.0f);
if (imu.calibated) {
processed_data.processed_imu_data[0] -= imu.imu_bias[0];
@@ -894,29 +909,23 @@ void Start_BMM350(void *argument)
int8_t rslt;
struct bmm350_mag_temp_data mag_data;
// ODR: 200HZ, AVG:
// ODR: 400HZ, AVG: 0
rslt = init_bmm(&dev);
if (rslt != BMM350_OK) {
printf("BMM350 initialization failed: %d\n", rslt);
}
//
rslt = bmm_configure_interrupt(&dev);
if (rslt != BMM350_OK) {
printf("Configuring interrupt failed: %d\n", rslt);
}
ulTaskNotifyTakeIndexed(BMM_NOTIFY_INT_DRDY, pdTRUE, 0);
ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, 0);
// osDelay(50);
/* Infinite loop */
for(;;)
{
// wait for the interrupt
// ulTaskNotifyTake(pdFALSE, osWaitForever);
// ulTaskNotifyTakeIndexed(pdFALSE, BMM_NOTIFY_INT_DRDY, osWaitForever);
// ulTaskNotifyTakeIndexed(BMM_NOTIFY_INT_DRDY, pdTRUE, osWaitForever);
ulTaskNotifyTakeIndexed(BMM_NOTIFY_INT_DRDY, pdTRUE, osWaitForever);
rslt = bmm350_get_compensated_mag_xyz_temp_data(&mag_data, &dev);
// ulTaskNotifyTake(pdFALSE, osWaitForever);
// ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, osWaitForever);
if (rslt == BMM350_OK)
{
osMessageQueuePut(Q_BMMHandle, &mag_data, 0, 5);