- Fixed IMU calibration function bugs

- 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

Core/Src/main.c

@@ -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);

Core/Src/sensors.c

@@ -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;
 }
 

Core/Src/trinity_helperlib.c

@@ -1,4 +1,12 @@
 #include "trinity_helperlib.h"
 
+void convert_rad_to_deg (float data[]) {
+	float factor = 0.01745329252f;
+
+	for (int i = 0; i <= sizeof(data)/sizeof(data[0]); i++) {
+		data[i] *= factor;
+	}
+}
+
 
 

Core/Src/trinity_helperlib.h

@@ -1,13 +1 @@
-/*
- * trinity_helperlib.h
- *
- *  Created on: Aug 17, 2026
- *      Author: pima
- */
-
-#ifndef SRC_TRINITY_HELPERLIB_H_
-#define SRC_TRINITY_HELPERLIB_H_
-
-
-
-#endif /* SRC_TRINITY_HELPERLIB_H_ */
+#define DEG2RAD 0.01745329252f