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/sensors.c
+26 -98
View File
@@ -1,18 +1,15 @@
#include "main.h"
#include "sensors.h"
//#include "stm32wbxx_hal_i2c.h"
#include "trinity_helperlib.h"
#include <stdio.h>
#include "cmsis_os2.h"
#include "task.h"
#define HAL_I2C_TIMEOUT 100
//extern osThreadId_t defaultTaskHandle;
//TaskHandle_t xStartDefaultTask = NULL;
//extern osThreadId_t defaultTaskHandle
volatile osThreadId_t xIMU_Task = NULL;
volatile osThreadId_t xMagn_Task = NULL;
//osThreadId_t xData_Task = NULL;
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
@@ -36,7 +33,6 @@ void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address) {
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
if (I2C_address->Instance == I2C1 && xMagn_Task != NULL) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
@@ -46,7 +42,6 @@ void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
if (hi2c->Instance == I2C1 && xMagn_Task != NULL) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
printf("ErrorCallback: %ld\n", HAL_I2C_GetError(hi2c));
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
@@ -55,12 +50,9 @@ void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
if (GPIO_Pin == GPIO_IMU_INT_Pin && xIMU_Task != NULL) {
// vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xIMU_Task, ICM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
// printf("Triggered ICM45686 EXTI\n");
}
if (GPIO_Pin == GPIO_MAGN_INT_Pin && xMagn_Task != NULL) {
// vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
}
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
@@ -160,16 +152,12 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
HAL_StatusTypeDef status;
uint8_t raw_data[15];
int16_t sensor_data[7] = {0};
int32_t sensor_data[7] = {0};
float processed_data[7] = {0};
uint8_t tx[15] = {0x00};
tx[0] = 0x00 | 0x80;
for (int i = 1; i <= samples; i++) {
// status = read_icm_dma(imu, tx, raw_data, sizeof(tx));
// osDelay(1);
// if (status != HAL_OK) {
// printf("Calibration read ICM failed: %d\n", status);
// }
ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, portMAX_DELAY);
status = read_icm_dma(imu, tx, raw_data, sizeof(tx));
if (status != HAL_OK) {
@@ -185,46 +173,34 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
sensor_data[6] += (int16_t)(raw_data[13] << 8 | raw_data[14]); // t
}
}
// The acc biases is set to 0 or 1 for gravity to avoid messy values without
// proper multiposition calibration done once per board/sensor
processed_data[0] = 0.0f; //(float)sensor_data[0] / ((float)samples * imu->acc_ssf);
processed_data[1] = 0.0f; //(float)sensor_data[1] / ((float)samples * imu->acc_ssf);
processed_data[2] = 1.0f; //(float)sensor_data[2] / ((float)samples * imu->acc_ssf);
processed_data[3] = (float)sensor_data[3] / ((float)samples * imu->gyro_ssf);
processed_data[4] = (float)sensor_data[4] / ((float)samples * imu->gyro_ssf);
processed_data[5] = (float)sensor_data[5] / ((float)samples * imu->gyro_ssf);
processed_data[6] = (float)sensor_data[6] / (float)samples;
imu->imu_bias[0] = (float)sensor_data[0] / (float)samples;
imu->imu_bias[1] = (float)sensor_data[1] / (float)samples;
imu->imu_bias[2] = (float)sensor_data[2] / (float)samples;
imu->imu_bias[3] = (float)sensor_data[3] / (float)samples;
imu->imu_bias[4] = (float)sensor_data[4] / (float)samples;
imu->imu_bias[5] = (float)sensor_data[5] / (float)samples;
imu->imu_bias[6] = (float)sensor_data[6] / (float)samples;
// remrove gravity
imu->imu_bias[2] -= 1.0f;
imu->calibated = true;
imu->imu_bias[0] = processed_data[0];
imu->imu_bias[1] = processed_data[1];
imu->imu_bias[2] = processed_data[2];// - 1.0f;
imu->imu_bias[3] = processed_data[3] * DEG2RAD;
imu->imu_bias[4] = processed_data[4] * DEG2RAD;
imu->imu_bias[5] = processed_data[5] * DEG2RAD;
imu->imu_bias[6] = processed_data[6] / 128.0f + 25.0f;
printf("ICM calibration bias: a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
imu->imu_bias[0], imu->imu_bias[1], imu->imu_bias[2],
imu->imu_bias[3], imu->imu_bias[4], imu->imu_bias[5],
imu->imu_bias[6]);
imu->calibated = true;
printf("ICM calibration bias: a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
imu->imu_bias[0], imu->imu_bias[1], imu->imu_bias[2],
imu->imu_bias[3], imu->imu_bias[4], imu->imu_bias[5],
imu->imu_bias[6]);
}
//void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
// uint8_t tx[2], rx[2] = {0x00, 0x00};
//
// tx[0] = ICM45686_INT1_CONFIG0;
// tx[1] = 0x04;
// HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
// if (status != HAL_OK) {
// printf("Enabling of IMU INT failed: %d\n", status);
// }
//
// tx[0] = ICM45686_INT1_CONFIG0 | 0x80;
// tx[1] = 0x00;
// status = read_icm(imu, tx, rx, sizeof(tx));
// if (status != HAL_OK) {
// printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
// } else {
// printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
// }
//}
void configure_and_enable_int_icm (ICM45686_HandleTypeDef *imu) {
uint8_t tx[2], rx[2] = {0x00, 0x00};
@@ -284,10 +260,8 @@ void configure_and_enable_int_icm (ICM45686_HandleTypeDef *imu) {
void bmm_init_DWT(void){
// Enable trace and debug block (TRCENA)
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
// Reset cycle counter
DWT->CYCCNT = 0;
// Enable the cycle counter
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}
@@ -295,66 +269,28 @@ void bmm_init_DWT(void){
BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr){
// Extract I2C device address from intf_ptr
uint8_t device_addr = *(uint8_t*)intf_ptr;
// STM32 HAL: Write register address, then read data
// Device address is left-shifted by 1 for STM32 HAL
// if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
if (HAL_I2C_Mem_Read_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length) == HAL_OK) {
// if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
if (ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
// if (ulTaskNotifyTakeIndexed(pdTRUE, BMM_NOTIFY_DMA, pdMS_TO_TICKS(10)) == pdTRUE) {
return BMM350_INTF_RET_SUCCESS; // 0
}
}
return -1; // Communication failure
}
//BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr) {
// // Extract I2C device address from intf_ptr
// uint8_t device_addr = *(uint8_t*)intf_ptr;
//
// // STM32 HAL: Write register address, then read data
// // Device address is left-shifted by 1 for STM32 HAL
// if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK)
// {
// return BMM350_INTF_RET_SUCCESS; // 0
// }
//
// return -1; // Communication failure
//}
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
uint8_t device_addr = *(uint8_t*)intf_ptr;
// uint8_t rslt = HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length);
// printf("rslt write: %d\n", rslt);
// // STM32 HAL: Write register address + data
if (HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length) == HAL_OK) {
// if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
// if (rslt == HAL_OK) {
// if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
if (ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
// if (ulTaskNotifyTakeIndexed(pdTRUE, BMM_NOTIFY_DMA, pdMS_TO_TICKS(10)) == pdTRUE) {
return BMM350_INTF_RET_SUCCESS; // 0
}
}
return -1;
}
//BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
// uint8_t device_addr = *(uint8_t*)intf_ptr;
//
// // STM32 HAL: Write register address + data
// if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
// return BMM350_INTF_RET_SUCCESS;
// }
//
// return -1;
//}
void bmm350_delay(uint32_t period_us, void *intf_ptr){
(void)intf_ptr; // Unused
// For STM32, use DWT cycle counter for accurate microsecond delays
uint32_t start = DWT->CYCCNT;
uint32_t cycles = period_us * (SystemCoreClock / 1000000);
@@ -375,8 +311,6 @@ int8_t init_bmm(struct bmm350_dev *dev) {
dev->delay_us = bmm350_delay;
dev->intf_ptr = &dev_addr;
// printf("Starting up and configuring BMM350\n");
// Initialize BMM350 driver
rslt = bmm350_init(dev);
if (rslt != BMM350_OK)
@@ -393,12 +327,6 @@ int8_t init_bmm(struct bmm350_dev *dev) {
rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_400HZ, BMM350_NO_AVERAGING, dev);
printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_400HZ, BMM350_NO_AVERAGING, rslt);
// // configure interrupt
// rslt = bmm350_enable_interrupt(BMM350_ENABLE_INTERRUPT, dev);
// printf("Interrupt enable: %d", rslt);
// rslt = bmm350_configure_interrupt(BMM350_LATCHED, BMM350_ACTIVE_HIGH, BMM350_INTR_OPEN_DRAIN, BMM350_MAP_TO_PIN, dev);
// printf("Configure interrupt: %d", rslt);
return BMM350_OK;
}