Fixed DMA and INT bugs

- Fixed indexed task notifications for INT and DMAf for both sensors
- Fixed the INT debugger startup freeze (added != NULL check in
callbacks)
- Fixed DMA notifications not being read properly/too early
- Changed IMU INT configure/enable function as one
- Added proper DMA checking in IMU calibration

Core/Inc/FreeRTOSConfig.h

@@ -76,6 +76,7 @@ extern uint32_t SystemCoreClock;
 #define configUSE_16_BIT_TICKS                   0
 #define configUSE_MUTEXES                        1
 #define configQUEUE_REGISTRY_SIZE                8
+#define configCHECK_FOR_STACK_OVERFLOW           2
 #define configUSE_RECURSIVE_MUTEXES              1
 #define configUSE_COUNTING_SEMAPHORES            1
 #define configUSE_PORT_OPTIMISED_TASK_SELECTION  0
@@ -95,6 +96,9 @@ extern uint32_t SystemCoreClock;
 #define configTIMER_QUEUE_LENGTH                 10
 #define configTIMER_TASK_STACK_DEPTH             1024
 
+/* The following flag must be enabled only when using newlib */
+#define configUSE_NEWLIB_REENTRANT          1
+
 /* CMSIS-RTOS V2 flags */
 #define configUSE_OS2_THREAD_SUSPEND_RESUME  1
 #define configUSE_OS2_THREAD_ENUMERATE       1
@@ -166,6 +170,7 @@ standard names. */
 
 /* USER CODE BEGIN Defines */
 /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
+#define configTASK_NOTIFICATION_ARRAY_ENTRIES    2
 /* USER CODE END Defines */
 
 #endif /* FREERTOS_CONFIG_H */

Core/Src/MadgwickAHRS.c

@@ -148,6 +148,14 @@ void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float
 	q[2] *= recipNorm;
 	q[3] *= recipNorm;
 
+	// enforce hemisphere continuity (because of the positive and negative doubling of output)
+	float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3];
+	if (dot < 0.0f){
+		q[0] = -q[0];
+		q[1] = -q[1];
+		q[2] = -q[2];
+		q[3] = -q[3];
+	}
     // Update filter state
     filter->q[0] = q[0];
     filter->q[1] = q[1];
@@ -226,6 +234,15 @@ void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, fl
 	q[2] *= recipNorm;
 	q[3] *= recipNorm;
 
+	// enforce hemisphere continuity (because of the positive and negative doubling of output)
+	float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3];
+	if (dot < 0.0f){
+		q[0] = -q[0];
+		q[1] = -q[1];
+		q[2] = -q[2];
+		q[3] = -q[3];
+	}
+
     // Update filter state
     filter->q[0] = q[0];
     filter->q[1] = q[1];

Core/Src/app_freertos.c

@@ -52,6 +52,20 @@
 
 /* USER CODE END FunctionPrototypes */
 
+/* Hook prototypes */
+void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName);
+
+/* USER CODE BEGIN 4 */
+void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName)
+{
+   /* Run time stack overflow checking is performed if
+   configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
+   called if a stack overflow is detected. */
+    __disable_irq();
+    for (;;) { } // breakpoint here
+}
+/* USER CODE END 4 */
+
 /* Private application code --------------------------------------------------*/
 /* USER CODE BEGIN Application */
 

Core/Src/main.c

@@ -44,6 +44,12 @@
 
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */
+//#define ICM_NOTIFY_INT_DRDY 0
+//#define ICM_NOTIFY_DMA 1
+//#define BMM_NOTIFY_INT_DRDY 0
+//#define BMM_NOTIFY_DMA 1
+//configASSERT(BMM_NOTIFY_DMA < configTASK_NOTIFICATION_ARRAY_ENTRIES);
+//configASSERT(BMM_NOTIFY_INT_DRDY < configTASK_NOTIFICATION_ARRAY_ENTRIES);
 
 /* USER CODE END PD */
 
@@ -359,7 +365,7 @@ static void MX_I2C1_Init(void)
 
   /* USER CODE END I2C1_Init 1 */
   hi2c1.Instance = I2C1;
-  hi2c1.Init.Timing = 0x10B17DB5;
+  hi2c1.Init.Timing = 0x00C12166;
   hi2c1.Init.OwnAddress1 = 0;
   hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
   hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
@@ -504,7 +510,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_64;
+  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
   hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
   hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
   hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
@@ -726,7 +732,7 @@ void Start_DataAnalysis(void *argument)
   // TODO: Einstellungen setzbar durch trinity control für IMU, Magnetometer, etc.
 
 //	HAL_StatusTypeDef status;
-	char output_buffer[128];
+//	char output_buffer[128];
 	ICM45686_Data imu_data;
 	struct bmm350_mag_temp_data magn_data;
 //	struct bmm350_raw_mag_data magn_data;
@@ -735,7 +741,7 @@ 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.15f);
+	MadgwickAHRS_init(&IMUQuat, 6400.0f, 0.00001f);
 
 	/* Infinite loop */
 	for(;;)
@@ -747,7 +753,7 @@ void Start_DataAnalysis(void *argument)
 	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
-//
+
 //	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)
 
@@ -805,8 +811,6 @@ void Start_ICM45686(void *argument)
 	HAL_StatusTypeDef status;
 //	char output_buffer[128];
 	xIMU_Task = osThreadGetId();
-	// ICM
-	//uint8_t usb_response;
 	uint8_t raw_imu_data[15];
 	int16_t imu_sensor_data[7];
 	ICM45686_Data processed_data;
@@ -815,30 +819,31 @@ void Start_ICM45686(void *argument)
 	imu.hspi = &hspi1;
 	imu.GPIO_Port = GPIO_SPI1_IMU_CS_GPIO_Port;
 	imu.GPIO_Pin = GPIO_SPI1_IMU_CS_Pin;
-	imu.acc_fs = ICM45686_ACC_FS_4G;
+	imu.acc_fs = ICM45686_ACC_FS_16G;
-	imu.acc_ssf = ICM45686_ACC_SSF[ICM45686_ACC_FS_4G >> 4];
+	imu.acc_ssf = ICM45686_ACC_SSF[ICM45686_ACC_FS_16G >> 4];
-	imu.gyro_fs = ICM45686_GYRO_FS_250DPS;
+	imu.gyro_fs = ICM45686_GYRO_FS_2000DPS;
-	imu.gyro_ssf = ICM45686_GYRO_SSF[ICM45686_GYRO_FS_250DPS >> 4];
+	imu.gyro_ssf = ICM45686_GYRO_SSF[ICM45686_GYRO_FS_2000DPS >> 4];
 	imu.odr = ICM45686_ODR_6_4kHz_LN ;
 
 	uint8_t tx[15] = {0x00};
 	tx[0] = 0x00 | 0x80;
+	uint16_t tx_len = sizeof(tx);
+
 
 	init_icm(&imu);
-	osDelay(50);
+	configure_and_enable_int_icm(&imu);
-	configure_interrupt_icm(&imu);
+	ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, 0);
-	enable_interrupt_icm(&imu);
+	ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, 0);
-//	osDelay(500);
+	calibrate_icm(&imu, 25000);
-//	calibrate_icm(&imu, 1000);
   /* Infinite loop */
   for(;;)
   {
-	  ulTaskNotifyTake(pdFALSE, osWaitForever);
+	  ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, portMAX_DELAY);
-	  status = read_icm_dma(&imu, tx, raw_imu_data, sizeof(tx));
+	  status = read_icm_dma(&imu, tx, raw_imu_data, tx_len);
 	  if (status != HAL_OK) {
 	    printf("Reading IMU sensordata failed: %d\n", status);
 	  }
-
+	  if (ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) > 0) {
 		  imu_sensor_data[0] = (int16_t)(raw_imu_data[1] << 8 | raw_imu_data[2]);    // a_x
 		  imu_sensor_data[1] = (int16_t)(raw_imu_data[3] << 8 | raw_imu_data[4]);    // a_y
 		  imu_sensor_data[2] = (int16_t)(raw_imu_data[5] << 8 | raw_imu_data[6]);    // a_z
@@ -862,11 +867,14 @@ void Start_ICM45686(void *argument)
 			  processed_data.processed_imu_data[3] -= imu.imu_bias[3];
 			  processed_data.processed_imu_data[4] -= imu.imu_bias[4];
 			  processed_data.processed_imu_data[5] -= imu.imu_bias[5];
-		  processed_data.processed_imu_data[6] -= imu.imu_bias[7];
+			  processed_data.processed_imu_data[6] -= imu.imu_bias[6];
-
 		  }
-
 		  osMessageQueuePut(Q_ICMHandle, &processed_data, 0, 5);
+	  } else {
+		  HAL_SPI_Abort(imu.hspi);
+		  HAL_GPIO_WritePin(imu.GPIO_Port, imu.GPIO_Pin, GPIO_PIN_SET);
+		  printf("IMU DMA data transaction timed out");
+	  }
   }
   /* USER CODE END Start_ICM45686 */
 }
@@ -881,44 +889,38 @@ void Start_ICM45686(void *argument)
 void Start_BMM350(void *argument)
 {
   /* USER CODE BEGIN Start_BMM350 */
-//	HAL_StatusTypeDef status;
-//	char output_buffer[128];
 	xMagn_Task = osThreadGetId();
-	// BMM
 	struct bmm350_dev dev = {0x00};
 	int8_t rslt;
 	struct bmm350_mag_temp_data mag_data;
-	osDelay(500);
-//	struct bmm350_raw_mag_data mag_data;
 
 	// ODR: 200HZ, AVG:
 	rslt = init_bmm(&dev);
 	if (rslt != BMM350_OK) {
 		printf("BMM350 initialization failed: %d\n", rslt);
-		printf("Chip id: 0x%02X\n", dev.chip_id);
 	}
-
+//
 	rslt = bmm_configure_interrupt(&dev);
 	if (rslt != BMM350_OK) {
 		printf("Configuring interrupt failed: %d\n", rslt);
 	}
-//	osDelay(200);
+	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);
+//	  ulTaskNotifyTake(pdFALSE, osWaitForever);
-      // magnetometer
+//	  ulTaskNotifyTakeIndexed(pdFALSE, BMM_NOTIFY_INT_DRDY, osWaitForever);
+//	  ulTaskNotifyTakeIndexed(BMM_NOTIFY_INT_DRDY, pdTRUE, osWaitForever);
       rslt = bmm350_get_compensated_mag_xyz_temp_data(&mag_data, &dev);
-//      rslt = bmm350_read_uncomp_mag_temp_data(&mag_data, &dev);
+//      ulTaskNotifyTake(pdFALSE, osWaitForever);
+//      ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, pdTRUE, osWaitForever);
       if (rslt == BMM350_OK)
       {
-//    	  sprintf(output_buffer, "X: %.2f uT, Y: %.2f uT, Z: %.2f uT, Temp: %.2f C\n",
-//                 mag_data.x, mag_data.y, mag_data.z, mag_data.temperature);
-//    	  CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
     	  osMessageQueuePut(Q_BMMHandle, &mag_data, 0, 5);
       }
-//      osDelay(100);
   }
   /* USER CODE END Start_BMM350 */
 }

Core/Src/sensors.c

@@ -10,65 +10,60 @@
 //extern osThreadId_t defaultTaskHandle;
 //TaskHandle_t xStartDefaultTask = NULL;
 //extern osThreadId_t defaultTaskHandle
-osThreadId_t xIMU_Task = NULL;
+volatile osThreadId_t xIMU_Task = NULL;
-osThreadId_t xMagn_Task = NULL;
+volatile osThreadId_t xMagn_Task = NULL;
 //osThreadId_t xData_Task = NULL;
 
 
 void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
-	 if (hspi->Instance == SPI1) {
+	 if (hspi->Instance == SPI1 && xIMU_Task != NULL) {
 		 HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
-
-		 // notify the task that the data is ready
 		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-		 vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
+		 vTaskNotifyGiveIndexedFromISR(xIMU_Task, ICM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
 		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 	 }
  }
 
 void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address) {
-//	printf("HAL_I2C_MemRxCpltCallback");
+	 if (I2C_address->Instance == I2C1 && xMagn_Task != NULL) {
-	 if (I2C_address->Instance == I2C1) {
 		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+//		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+		 vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
 		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 	 }
-
  }
+
 void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
-//	printf("HAL_I2C_MemTxCpltCallback");
+	 if (I2C_address->Instance == I2C1 && xMagn_Task != NULL) {
-	 if (I2C_address->Instance == I2C1) {
 		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+//		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+		 vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
 		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
 	 }
  }
 
 void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
-    if (hi2c->Instance == I2C1) {
+    if (hi2c->Instance == I2C1 && xMagn_Task != NULL) {
         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
         printf("ErrorCallback: %ld\n", HAL_I2C_GetError(hi2c));
-        vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+//        vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+        vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
         portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
     }
 }
 
 void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
-	// was macht das?
 	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-
+	if (GPIO_Pin == GPIO_IMU_INT_Pin && xIMU_Task != NULL) {
-	if (GPIO_Pin == GPIO_IMU_INT_Pin) {
+//		 vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
-		 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) {
-	if (GPIO_Pin == GPIO_MAGN_INT_Pin) {
+//		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
-		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
+		 vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
-//		 printf("Triggered BMM350 EXTI\n");
 	}
-	// was macht das?
 	portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
-
 }
 
 // ICM 45686
@@ -93,33 +88,10 @@ HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint16_t
 }
 
 void init_icm(ICM45686_HandleTypeDef *imu) {
-	// TODO: Add settings as enum to choose from
-	// FIFO
-	// Interrupt
-	/* Interrupt functionality is configured via the Interrupt Configuration register. Items that are configurable include the
-		interrupt pins configuration, the interrupt latching and clearing method, and triggers for the interrupt. Items that
-		can trigger an interrupt are (1) Clock generator locked to new reference oscillator (used when switching clock
-		sources); (2) new data is available to be read (from the FIFO and Data registers); (3) accelerometer event
-		interrupts; (4) FIFO watermark; (5) FIFO overflow. The interrupt status can be read from the Interrupt Status
-		register.
-		- Configure interrupt pin on icm
-		- configure interrupt pin on bmm
-		- -> push/pull or open drain, latch, polarity
-		- everything stays as is only that a interrupt callback gets added, that when called wakes up the
-		corresponding task for the sensor
-		*/
-	// APEX???
-	// EDMP???
-
 	uint8_t tx[2], rx[2];
 	HAL_StatusTypeDef status;
-
+//	// WHO AM
-	printf("Initializing ICM45686\n");
-	// WHO AM
 	tx[0] = ICM45686_WHO_AM_I | 0x80;
-//	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = read_icm(imu, tx, rx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error in reading WHO AM I:%d\n", status);
@@ -127,12 +99,9 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
 	  printf("WHO AM I (0xE9): 0x%02X \n", rx[1]);
 	}
 
-	// acc
+	// acc config
 	tx[0] = ICM45686_ACCEL_CONFIG0;
 	tx[1] = imu->acc_fs | imu->odr;
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = write_icm(imu, tx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error in setting ACCEL_CONFIG0:%d\n", status);
@@ -142,9 +111,6 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
 
 	tx[0] = ICM45686_ACCEL_CONFIG0 | 0x80;
 	tx[1] = 0x00;
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = read_icm(imu, tx, rx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error reading ACCEL_CONFIG0:%d\n", status);
@@ -152,12 +118,9 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
 	  printf("ACCEL_CONFIG0 read: 0x%02X \n", rx[1]);
 	}
 
-	// gyro
+	// gyro config
 	tx[0] = ICM45686_GYRO_CONFIG0;
 	tx[1] = imu->gyro_fs | imu->odr;
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = write_icm(imu, tx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error in setting GYRO_CONFIG0:%d\n", status);
@@ -167,9 +130,6 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
 
 	tx[0] = ICM45686_GYRO_CONFIG0 | 0x80;
 	tx[1] = 0x00;
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = read_icm(imu, tx, rx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error reading GYRO_CONFIG0:%d\n", status);
@@ -180,9 +140,6 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
 	// power settings
 	tx[0] = ICM45686_PWR_MGMT0;
 	tx[1] =  0x0F; //0x10 | 0x03;
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = write_icm(imu, tx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error in setting PWR_MGMT0:%d\n", status);
@@ -192,17 +149,12 @@ void init_icm(ICM45686_HandleTypeDef *imu) {
 
 	tx[0] = ICM45686_PWR_MGMT0 | 0x80;
 	tx[1] = 0x00;
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
-//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
-//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
 	status = read_icm(imu, tx, rx, sizeof(tx));
 	if (status != HAL_OK) {
 	  printf("Error reading PWR_MGMT0:%d\n", status);
 	} else {
 	  printf("PWR_MGMT0 read: 0x%02X \n", rx[1]);
 	}
-
-	osDelay(250);
 }
 
 void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
@@ -213,10 +165,17 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
 	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) {
 		printf("Calibration read ICM failed: %d\n", status);
 		}
+		if (ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) > 0) {
 		  sensor_data[0] += (int16_t)(raw_data[1] << 8 | raw_data[2]);    // a_x
 		  sensor_data[1] += (int16_t)(raw_data[3] << 8 | raw_data[4]);    // a_y
 		  sensor_data[2] += (int16_t)(raw_data[5] << 8 | raw_data[6]);    // a_z
@@ -225,6 +184,7 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
 		  sensor_data[5] += (int16_t)(raw_data[11] << 8 | raw_data[12]);  // g_z
 		  sensor_data[6] += (int16_t)(raw_data[13] << 8 | raw_data[14]);  // t
 		}
+	}
 
 	  imu->imu_bias[0] = (float)sensor_data[0] / (float)samples;
 	  imu->imu_bias[1] = (float)sensor_data[1] / (float)samples;
@@ -239,20 +199,38 @@ void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
 
 	  imu->calibated = true;
 
-	  printf("Calibration finished of ICM45686\n");
+	  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",
-	  printf("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) {
+//void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
-	uint8_t tx[2], rx[2] = {0x00, 0x00};
+//	uint8_t tx[2], rx[2] = {0x00, 0x00};
-	tx[0] = ICM45686_INT1_CONFIG0;
+//
-	tx[1] = 0x04;
+//	tx[0] = ICM45686_INT1_CONFIG0;
-	printf("Enabling IMU INT pin\n");
+//	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};
+
+	// turn off int
+	tx[0] = ICM45686_INT1_CONFIG0;
+	tx[1] = 0x00;
 	HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
 	if (status != HAL_OK) {
 		printf("Enabling of IMU INT failed: %d\n", status);
@@ -262,19 +240,15 @@ void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
 	tx[1] = 0x00;
 	status = read_icm(imu, tx, rx, sizeof(tx));
 	if (status != HAL_OK) {
-		printf("Reading IMU INT configuration0 failed: %d\n", status);
+		printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
 	} else {
 		printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
 	}
-}
-
-void configure_interrupt_icm (ICM45686_HandleTypeDef *imu) {
-	uint8_t tx[2], rx[2] = {0x00, 0x00};
 
+	// configure int1 to push-pull, pulse, active high
 	tx[0] = ICM45686_INT1_CONFIG2;
 	tx[1] = 0x01;
-	printf("Configuring IMU INT\n");
+	status = write_icm(imu, tx, sizeof(tx));
-	HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
 	if (status != HAL_OK) {
 		printf("Configuration of IMU INT failed: %d\n", status);
 	}
@@ -283,10 +257,27 @@ void configure_interrupt_icm (ICM45686_HandleTypeDef *imu) {
 	tx[1] = 0x00;
 	status = read_icm(imu, tx, rx, sizeof(tx));
 	if (status != HAL_OK) {
-		printf("Reading IMU INT configuration2 failed: %d\n", status);
+		printf("Reading IMU INT1_CONFIG2 failed: %d\n", status);
 	} else {
 		printf("INT1_CONFIG2 read: 0x%02X\n", rx[1]);
 	}
+
+	// enable int
+	tx[0] = ICM45686_INT1_CONFIG0;
+	tx[1] = 0x04;
+	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]);
+	}
 }
 
 // BMM350
@@ -309,7 +300,9 @@ BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32
 //     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 (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
     	}
     }
@@ -317,6 +310,20 @@ BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32
 
 }
 
+//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);
@@ -325,13 +332,26 @@ BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_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 (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
 
@@ -355,7 +375,7 @@ 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");
+//    printf("Starting up and configuring BMM350\n");
 
     // Initialize BMM350 driver
     rslt = bmm350_init(dev);
@@ -370,8 +390,8 @@ int8_t init_bmm(struct bmm350_dev *dev) {
     // Set powermode, odr and avg
     rslt = bmm350_set_powermode(BMM350_NORMAL_MODE, dev);
     printf("Powermode (%d): %d\n", BMM350_NORMAL_MODE, rslt);
-    rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, 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_200HZ, BMM350_AVERAGING_2, rslt);
+    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);

Core/Src/sensors.h

@@ -10,10 +10,17 @@
 // I2C Handle
 extern I2C_HandleTypeDef hi2c1;
 extern osThreadId_t defaultTaskHandle;
-extern osThreadId_t xIMU_Task;
+extern volatile osThreadId_t xIMU_Task;
-extern osThreadId_t xMagn_Task;
+extern volatile osThreadId_t xMagn_Task;
 //extern osThreadId_t xData_Task;
 
+typedef enum {
+	ICM_NOTIFY_INT_DRDY = 0,
+	ICM_NOTIFY_DMA = 1,
+	BMM_NOTIFY_INT_DRDY = 0,
+	BMM_NOTIFY_DMA = 1
+} SENSOR_NOTIFY_INDEX;
+
 // ICM45686
 typedef struct {
 	SPI_HandleTypeDef *hspi;
@@ -66,7 +73,8 @@ typedef enum {
 	ICM45686_FIFO_COUNT = 0x12,
 	ICM45686_FIFO_DATA = 0x14,
 	ICM45686_INT1_CONFIG2 = 0x18,
-	ICM45686_INT1_CONFIG0 = 0x16
+	ICM45686_INT1_CONFIG0 = 0x16,
+	ICM45686_INT1_STATUS0 = 0x19
 } ICM45686_registers;
 
 typedef enum {
@@ -149,7 +157,7 @@ HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8
 HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint16_t len_tx);
 void calibrate_icm (ICM45686_HandleTypeDef * imu, int samples);
 void enable_interrupt_icm(ICM45686_HandleTypeDef *imu);
-void configure_interrupt_icm (ICM45686_HandleTypeDef *imu);
+void configure_and_enable_int_icm (ICM45686_HandleTypeDef *imu);
 
 // BMM350 - Declarations of read/write/delay functions for the BMM350 API
 void bmm_init_DWT(void);

Core/Src/stm32wbxx_hal_msp.c

@@ -125,10 +125,14 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
     GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
     GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
     HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
+    __HAL_SYSCFG_FASTMODEPLUS_ENABLE(SYSCFG_FASTMODEPLUS_PB8);
+
+    __HAL_SYSCFG_FASTMODEPLUS_ENABLE(SYSCFG_FASTMODEPLUS_PB9);
+
     /* Peripheral clock enable */
     __HAL_RCC_I2C1_CLK_ENABLE();
 

Debug/Core/Src/MadgwickAHRS.cyclo

@@ -1,4 +1,4 @@
 ../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init	1
-../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update	7
+../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update	8
-../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU	4
+../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU	5
-../Core/Src/MadgwickAHRS.c:240:7:invSqrt	1
+../Core/Src/MadgwickAHRS.c:257:7:invSqrt	1

Debug/Core/Src/MadgwickAHRS.su

@@ -1,4 +1,4 @@
 ../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init	24	static
 ../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update	208	static
-../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU	144	static
+../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU	152	static
-../Core/Src/MadgwickAHRS.c:240:7:invSqrt	32	static
+../Core/Src/MadgwickAHRS.c:257:7:invSqrt	32	static

Debug/Core/Src/app_freertos.cyclo

@@ -0,0 +1 @@
+../Core/Src/app_freertos.c:59:6:vApplicationStackOverflowHook	1

Debug/Core/Src/app_freertos.d

@@ -6,6 +6,7 @@ Core/Src/app_freertos.o: ../Core/Src/app_freertos.c \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Core/Inc/main.h ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_hal.h \
@@ -76,6 +77,7 @@ Core/Src/app_freertos.o: ../Core/Src/app_freertos.c \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Core/Inc/main.h:

Debug/Core/Src/app_freertos.su

@@ -0,0 +1 @@
+../Core/Src/app_freertos.c:59:6:vApplicationStackOverflowHook	16	static,ignoring_inline_asm

Debug/Core/Src/freertos_port.d

@@ -66,6 +66,7 @@ Core/Src/freertos_port.o: ../Core/Src/freertos_port.c \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Utilities/lpm/tiny_lpm/stm32_lpm.h
@@ -139,6 +140,7 @@ Core/Src/freertos_port.o: ../Core/Src/freertos_port.c \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Utilities/lpm/tiny_lpm/stm32_lpm.h:

Debug/Core/Src/main.cyclo

@@ -3,21 +3,21 @@
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:364:22:LL_AHB1_GRP1_EnableClock	1
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:539:22:LL_AHB2_GRP1_EnableClock	1
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_hsem.h:233:26:LL_HSEM_1StepLock	2
-../Core/Src/main.c:132:5:_write	2
+../Core/Src/main.c:138:5:_write	2
-../Core/Src/main.c:145:5:main	2
+../Core/Src/main.c:151:5:main	2
-../Core/Src/main.c:260:6:SystemClock_Config	3
+../Core/Src/main.c:266:6:SystemClock_Config	3
-../Core/Src/main.c:326:6:PeriphCommonClock_Config	2
+../Core/Src/main.c:332:6:PeriphCommonClock_Config	2
-../Core/Src/main.c:351:13:MX_I2C1_Init	4
+../Core/Src/main.c:357:13:MX_I2C1_Init	4
-../Core/Src/main.c:399:13:MX_IPCC_Init	2
+../Core/Src/main.c:405:13:MX_IPCC_Init	2
-../Core/Src/main.c:425:13:MX_RF_Init	1
+../Core/Src/main.c:431:13:MX_RF_Init	1
-../Core/Src/main.c:446:13:MX_RTC_Init	3
+../Core/Src/main.c:452:13:MX_RTC_Init	3
-../Core/Src/main.c:489:13:MX_SPI1_Init	2
+../Core/Src/main.c:495:13:MX_SPI1_Init	2
-../Core/Src/main.c:529:13:MX_SPI2_Init	2
+../Core/Src/main.c:535:13:MX_SPI2_Init	2
-../Core/Src/main.c:569:13:MX_TIM2_Init	6
+../Core/Src/main.c:575:13:MX_TIM2_Init	6
-../Core/Src/main.c:624:13:MX_DMA_Init	1
+../Core/Src/main.c:630:13:MX_DMA_Init	1
-../Core/Src/main.c:652:13:MX_GPIO_Init	1
+../Core/Src/main.c:658:13:MX_GPIO_Init	1
-../Core/Src/main.c:719:6:Start_DataAnalysis	1
+../Core/Src/main.c:725:6:Start_DataAnalysis	1
-../Core/Src/main.c:802:6:Start_ICM45686	3
+../Core/Src/main.c:808:6:Start_ICM45686	4
-../Core/Src/main.c:881:6:Start_BMM350	4
+../Core/Src/main.c:896:6:Start_BMM350	4
-../Core/Src/main.c:934:6:HAL_TIM_PeriodElapsedCallback	2
+../Core/Src/main.c:943:6:HAL_TIM_PeriodElapsedCallback	2
-../Core/Src/main.c:952:6:Error_Handler	1
+../Core/Src/main.c:961:6:Error_Handler	1

Debug/Core/Src/main.d

@@ -68,6 +68,7 @@ Core/Src/main.o: ../Core/Src/main.c ../Core/Inc/main.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../USB_Device/App/usb_device.h \
  ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h \
  ../USB_Device/Target/usbd_conf.h ../Core/Src/MadgwickAHRS.h \
@@ -155,6 +156,7 @@ Core/Src/main.o: ../Core/Src/main.c ../Core/Inc/main.h \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../USB_Device/App/usb_device.h:
 ../Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h:
 ../USB_Device/Target/usbd_conf.h:

Debug/Core/Src/main.su

@@ -3,21 +3,21 @@
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:364:22:LL_AHB1_GRP1_EnableClock	24	static
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:539:22:LL_AHB2_GRP1_EnableClock	24	static
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_hsem.h:233:26:LL_HSEM_1StepLock	16	static
-../Core/Src/main.c:132:5:_write	32	static
+../Core/Src/main.c:138:5:_write	32	static
-../Core/Src/main.c:145:5:main	8	static
+../Core/Src/main.c:151:5:main	8	static
-../Core/Src/main.c:260:6:SystemClock_Config	112	static
+../Core/Src/main.c:266:6:SystemClock_Config	112	static
-../Core/Src/main.c:326:6:PeriphCommonClock_Config	88	static
+../Core/Src/main.c:332:6:PeriphCommonClock_Config	88	static
-../Core/Src/main.c:351:13:MX_I2C1_Init	8	static
+../Core/Src/main.c:357:13:MX_I2C1_Init	8	static
-../Core/Src/main.c:399:13:MX_IPCC_Init	8	static
+../Core/Src/main.c:405:13:MX_IPCC_Init	8	static
-../Core/Src/main.c:425:13:MX_RF_Init	4	static
+../Core/Src/main.c:431:13:MX_RF_Init	4	static
-../Core/Src/main.c:446:13:MX_RTC_Init	8	static
+../Core/Src/main.c:452:13:MX_RTC_Init	8	static
-../Core/Src/main.c:489:13:MX_SPI1_Init	8	static
+../Core/Src/main.c:495:13:MX_SPI1_Init	8	static
-../Core/Src/main.c:529:13:MX_SPI2_Init	8	static
+../Core/Src/main.c:535:13:MX_SPI2_Init	8	static
-../Core/Src/main.c:569:13:MX_TIM2_Init	48	static
+../Core/Src/main.c:575:13:MX_TIM2_Init	48	static
-../Core/Src/main.c:624:13:MX_DMA_Init	8	static
+../Core/Src/main.c:630:13:MX_DMA_Init	8	static
-../Core/Src/main.c:652:13:MX_GPIO_Init	32	static
+../Core/Src/main.c:658:13:MX_GPIO_Init	32	static
-../Core/Src/main.c:719:6:Start_DataAnalysis	208	static
+../Core/Src/main.c:725:6:Start_DataAnalysis	80	static
-../Core/Src/main.c:802:6:Start_ICM45686	152	static
+../Core/Src/main.c:808:6:Start_ICM45686	152	static
-../Core/Src/main.c:881:6:Start_BMM350	216	static
+../Core/Src/main.c:896:6:Start_BMM350	216	static
-../Core/Src/main.c:934:6:HAL_TIM_PeriodElapsedCallback	16	static
+../Core/Src/main.c:943:6:HAL_TIM_PeriodElapsedCallback	16	static
-../Core/Src/main.c:952:6:Error_Handler	4	static,ignoring_inline_asm
+../Core/Src/main.c:961:6:Error_Handler	4	static,ignoring_inline_asm

Debug/Core/Src/sensors.cyclo

@@ -1,18 +1,17 @@
-../Core/Src/sensors.c:18:6:HAL_SPI_TxRxCpltCallback	3
+../Core/Src/sensors.c:18:6:HAL_SPI_TxRxCpltCallback	4
-../Core/Src/sensors.c:29:6:HAL_I2C_MemRxCpltCallback	3
+../Core/Src/sensors.c:27:6:HAL_I2C_MemRxCpltCallback	4
-../Core/Src/sensors.c:38:6:HAL_I2C_MemTxCpltCallback	3
+../Core/Src/sensors.c:36:6:HAL_I2C_MemTxCpltCallback	4
-../Core/Src/sensors.c:47:6:HAL_I2C_ErrorCallback	3
+../Core/Src/sensors.c:45:6:HAL_I2C_ErrorCallback	4
-../Core/Src/sensors.c:56:6:HAL_GPIO_EXTI_Callback	4
+../Core/Src/sensors.c:55:6:HAL_GPIO_EXTI_Callback	6
-../Core/Src/sensors.c:75:19:read_icm	1
+../Core/Src/sensors.c:70:19:read_icm	1
-../Core/Src/sensors.c:82:19:read_icm_dma	1
+../Core/Src/sensors.c:77:19:read_icm_dma	1
-../Core/Src/sensors.c:88:19:write_icm	1
+../Core/Src/sensors.c:83:19:write_icm	1
-../Core/Src/sensors.c:95:6:init_icm	8
+../Core/Src/sensors.c:90:6:init_icm	8
-../Core/Src/sensors.c:208:6:calibrate_icm	3
+../Core/Src/sensors.c:160:6:calibrate_icm	4
-../Core/Src/sensors.c:250:6:enable_interrupt_icm	3
+../Core/Src/sensors.c:228:6:configure_and_enable_int_icm	7
-../Core/Src/sensors.c:271:6:configure_interrupt_icm	3
+../Core/Src/sensors.c:284:6:bmm_init_DWT	1
-../Core/Src/sensors.c:293:6:bmm_init_DWT	1
+../Core/Src/sensors.c:295:22:bmm350_i2c_read	3
-../Core/Src/sensors.c:304:22:bmm350_i2c_read	3
+../Core/Src/sensors.c:327:22:bmm350_i2c_write	3
-../Core/Src/sensors.c:320:22:bmm350_i2c_write	3
+../Core/Src/sensors.c:355:6:bmm350_delay	2
-../Core/Src/sensors.c:335:6:bmm350_delay	2
+../Core/Src/sensors.c:366:8:init_bmm	2
-../Core/Src/sensors.c:346:8:init_bmm	2
+../Core/Src/sensors.c:405:8:bmm_configure_interrupt	1
-../Core/Src/sensors.c:385:8:bmm_configure_interrupt	1

Debug/Core/Src/sensors.d

@@ -68,6 +68,7 @@ Core/Src/sensors.o: ../Core/Src/sensors.c ../Core/Inc/main.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Drivers/CMSIS/RTOS2/Include/cmsis_os2.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h
@@ -147,6 +148,7 @@ Core/Src/sensors.o: ../Core/Src/sensors.c ../Core/Inc/main.h \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Drivers/CMSIS/RTOS2/Include/cmsis_os2.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:

Debug/Core/Src/sensors.su

@@ -1,18 +1,17 @@
 ../Core/Src/sensors.c:18:6:HAL_SPI_TxRxCpltCallback	24	static,ignoring_inline_asm
-../Core/Src/sensors.c:29:6:HAL_I2C_MemRxCpltCallback	24	static,ignoring_inline_asm
+../Core/Src/sensors.c:27:6:HAL_I2C_MemRxCpltCallback	24	static,ignoring_inline_asm
-../Core/Src/sensors.c:38:6:HAL_I2C_MemTxCpltCallback	24	static,ignoring_inline_asm
+../Core/Src/sensors.c:36:6:HAL_I2C_MemTxCpltCallback	24	static,ignoring_inline_asm
-../Core/Src/sensors.c:47:6:HAL_I2C_ErrorCallback	24	static,ignoring_inline_asm
+../Core/Src/sensors.c:45:6:HAL_I2C_ErrorCallback	24	static,ignoring_inline_asm
-../Core/Src/sensors.c:56:6:HAL_GPIO_EXTI_Callback	24	static,ignoring_inline_asm
+../Core/Src/sensors.c:55:6:HAL_GPIO_EXTI_Callback	24	static,ignoring_inline_asm
-../Core/Src/sensors.c:75:19:read_icm	40	static
+../Core/Src/sensors.c:70:19:read_icm	40	static
-../Core/Src/sensors.c:82:19:read_icm_dma	32	static
+../Core/Src/sensors.c:77:19:read_icm_dma	32	static
-../Core/Src/sensors.c:88:19:write_icm	32	static
+../Core/Src/sensors.c:83:19:write_icm	32	static
-../Core/Src/sensors.c:95:6:init_icm	24	static
+../Core/Src/sensors.c:90:6:init_icm	24	static
-../Core/Src/sensors.c:208:6:calibrate_icm	168	static
+../Core/Src/sensors.c:160:6:calibrate_icm	168	static
-../Core/Src/sensors.c:250:6:enable_interrupt_icm	24	static
+../Core/Src/sensors.c:228:6:configure_and_enable_int_icm	24	static
-../Core/Src/sensors.c:271:6:configure_interrupt_icm	24	static
+../Core/Src/sensors.c:284:6:bmm_init_DWT	4	static
-../Core/Src/sensors.c:293:6:bmm_init_DWT	4	static
+../Core/Src/sensors.c:295:22:bmm350_i2c_read	40	static
-../Core/Src/sensors.c:304:22:bmm350_i2c_read	40	static
+../Core/Src/sensors.c:327:22:bmm350_i2c_write	40	static
-../Core/Src/sensors.c:320:22:bmm350_i2c_write	40	static
+../Core/Src/sensors.c:355:6:bmm350_delay	24	static
-../Core/Src/sensors.c:335:6:bmm350_delay	24	static
+../Core/Src/sensors.c:366:8:init_bmm	24	static
-../Core/Src/sensors.c:346:8:init_bmm	24	static
+../Core/Src/sensors.c:405:8:bmm_configure_interrupt	32	static
-../Core/Src/sensors.c:385:8:bmm_configure_interrupt	32	static

Debug/Core/Src/stm32wbxx_hal_msp.cyclo

@@ -7,15 +7,16 @@
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:1038:22:LL_APB1_GRP1_DisableClock	1
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:1274:22:LL_APB2_GRP1_EnableClock	1
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:1328:22:LL_APB2_GRP1_DisableClock	1
+../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_system.h:567:22:LL_SYSCFG_EnableFastModePlus	1
 ../Core/Src/stm32wbxx_hal_msp.c:71:6:HAL_MspInit	1
 ../Core/Src/stm32wbxx_hal_msp.c:101:6:HAL_I2C_MspInit	5
-../Core/Src/stm32wbxx_hal_msp.c:189:6:HAL_I2C_MspDeInit	2
+../Core/Src/stm32wbxx_hal_msp.c:193:6:HAL_I2C_MspDeInit	2
-../Core/Src/stm32wbxx_hal_msp.c:227:6:HAL_IPCC_MspInit	2
+../Core/Src/stm32wbxx_hal_msp.c:231:6:HAL_IPCC_MspInit	2
-../Core/Src/stm32wbxx_hal_msp.c:255:6:HAL_IPCC_MspDeInit	2
+../Core/Src/stm32wbxx_hal_msp.c:259:6:HAL_IPCC_MspDeInit	2
-../Core/Src/stm32wbxx_hal_msp.c:281:6:HAL_RTC_MspInit	3
+../Core/Src/stm32wbxx_hal_msp.c:285:6:HAL_RTC_MspInit	3
-../Core/Src/stm32wbxx_hal_msp.c:323:6:HAL_RTC_MspDeInit	2
+../Core/Src/stm32wbxx_hal_msp.c:327:6:HAL_RTC_MspDeInit	2
-../Core/Src/stm32wbxx_hal_msp.c:349:6:HAL_SPI_MspInit	5
+../Core/Src/stm32wbxx_hal_msp.c:353:6:HAL_SPI_MspInit	5
-../Core/Src/stm32wbxx_hal_msp.c:461:6:HAL_SPI_MspDeInit	3
+../Core/Src/stm32wbxx_hal_msp.c:465:6:HAL_SPI_MspDeInit	3
-../Core/Src/stm32wbxx_hal_msp.c:515:6:HAL_TIM_PWM_MspInit	2
+../Core/Src/stm32wbxx_hal_msp.c:519:6:HAL_TIM_PWM_MspInit	2
-../Core/Src/stm32wbxx_hal_msp.c:532:6:HAL_TIM_MspPostInit	2
+../Core/Src/stm32wbxx_hal_msp.c:536:6:HAL_TIM_MspPostInit	2
-../Core/Src/stm32wbxx_hal_msp.c:566:6:HAL_TIM_PWM_MspDeInit	2
+../Core/Src/stm32wbxx_hal_msp.c:570:6:HAL_TIM_PWM_MspDeInit	2

Debug/Core/Src/stm32wbxx_hal_msp.su

@@ -7,15 +7,16 @@
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:1038:22:LL_APB1_GRP1_DisableClock	16	static
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:1274:22:LL_APB2_GRP1_EnableClock	24	static
 ../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_bus.h:1328:22:LL_APB2_GRP1_DisableClock	16	static
+../Drivers/STM32WBxx_HAL_Driver/Inc/stm32wbxx_ll_system.h:567:22:LL_SYSCFG_EnableFastModePlus	16	static
 ../Core/Src/stm32wbxx_hal_msp.c:71:6:HAL_MspInit	8	static
 ../Core/Src/stm32wbxx_hal_msp.c:101:6:HAL_I2C_MspInit	120	static
-../Core/Src/stm32wbxx_hal_msp.c:189:6:HAL_I2C_MspDeInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:193:6:HAL_I2C_MspDeInit	16	static
-../Core/Src/stm32wbxx_hal_msp.c:227:6:HAL_IPCC_MspInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:231:6:HAL_IPCC_MspInit	16	static
-../Core/Src/stm32wbxx_hal_msp.c:255:6:HAL_IPCC_MspDeInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:259:6:HAL_IPCC_MspDeInit	16	static
-../Core/Src/stm32wbxx_hal_msp.c:281:6:HAL_RTC_MspInit	96	static
+../Core/Src/stm32wbxx_hal_msp.c:285:6:HAL_RTC_MspInit	96	static
-../Core/Src/stm32wbxx_hal_msp.c:323:6:HAL_RTC_MspDeInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:327:6:HAL_RTC_MspDeInit	16	static
-../Core/Src/stm32wbxx_hal_msp.c:349:6:HAL_SPI_MspInit	40	static
+../Core/Src/stm32wbxx_hal_msp.c:353:6:HAL_SPI_MspInit	40	static
-../Core/Src/stm32wbxx_hal_msp.c:461:6:HAL_SPI_MspDeInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:465:6:HAL_SPI_MspDeInit	16	static
-../Core/Src/stm32wbxx_hal_msp.c:515:6:HAL_TIM_PWM_MspInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:519:6:HAL_TIM_PWM_MspInit	16	static
-../Core/Src/stm32wbxx_hal_msp.c:532:6:HAL_TIM_MspPostInit	40	static
+../Core/Src/stm32wbxx_hal_msp.c:536:6:HAL_TIM_MspPostInit	40	static
-../Core/Src/stm32wbxx_hal_msp.c:566:6:HAL_TIM_PWM_MspDeInit	16	static
+../Core/Src/stm32wbxx_hal_msp.c:570:6:HAL_TIM_PWM_MspDeInit	16	static

Debug/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.cyclo

@@ -80,5 +80,6 @@
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2833:14:CreateBlock	2
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2850:14:AllocBlock	2
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2867:13:FreeBlock	1
+../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2919:13:vApplicationStackOverflowHook	1
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2934:13:vApplicationGetIdleTaskMemory	1
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2948:13:vApplicationGetTimerTaskMemory	1

Debug/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h \
@@ -71,6 +72,7 @@ Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/event_groups.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.su

@@ -80,5 +80,6 @@
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2833:14:CreateBlock	24	static
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2850:14:AllocBlock	24	static
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2867:13:FreeBlock	24	static
+../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2919:13:vApplicationStackOverflowHook	24	static,ignoring_inline_asm
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2934:13:vApplicationGetIdleTaskMemory	24	static
 ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c:2948:13:vApplicationGetTimerTaskMemory	24	static

Debug/Middlewares/Third_Party/FreeRTOS/Source/croutine.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/croutine.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h
@@ -17,6 +18,7 @@ Middlewares/Third_Party/FreeRTOS/Source/croutine.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/croutine.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/event_groups.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/event_groups.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/timers.h \
@@ -20,6 +21,7 @@ Middlewares/Third_Party/FreeRTOS/Source/event_groups.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/timers.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/list.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/list.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h
 ../Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h:
 ../Core/Inc/FreeRTOSConfig.h:
@@ -15,4 +16,5 @@ Middlewares/Third_Party/FreeRTOS/Source/list.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h
 ../Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h:
@@ -16,5 +17,6 @@ Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h
 ../Middlewares/Third_Party/FreeRTOS/Source/include/FreeRTOS.h:
@@ -16,5 +17,6 @@ Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/queue.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/queue.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/queue.h \
@@ -18,6 +19,7 @@ Middlewares/Third_Party/FreeRTOS/Source/queue.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/queue.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h
@@ -17,6 +18,7 @@ Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/stream_buffer.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/tasks.cyclo

@@ -25,7 +25,7 @@
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:2550:17:uxTaskGetSystemState	4
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:2677:12:xTaskCatchUpTicks	2
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:2781:12:xTaskIncrementTick	14
-../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3070:6:vTaskSwitchContext	5
+../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3070:6:vTaskSwitchContext	9
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3144:6:vTaskPlaceOnEventList	2
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3169:6:vTaskPlaceOnUnorderedEventList	3
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3197:10:vTaskPlaceOnEventListRestricted	3

Debug/Middlewares/Third_Party/FreeRTOS/Source/tasks.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/tasks.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/timers.h \
@@ -19,6 +20,7 @@ Middlewares/Third_Party/FreeRTOS/Source/tasks.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/timers.h:

Debug/Middlewares/Third_Party/FreeRTOS/Source/tasks.su

@@ -25,7 +25,7 @@
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:2550:17:uxTaskGetSystemState	32	static
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:2677:12:xTaskCatchUpTicks	24	static,ignoring_inline_asm
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:2781:12:xTaskIncrementTick	48	static,ignoring_inline_asm
-../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3070:6:vTaskSwitchContext	24	static,ignoring_inline_asm
+../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3070:6:vTaskSwitchContext	32	static,ignoring_inline_asm
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3144:6:vTaskPlaceOnEventList	24	static,ignoring_inline_asm
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3169:6:vTaskPlaceOnUnorderedEventList	40	static,ignoring_inline_asm
 ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c:3197:10:vTaskPlaceOnEventListRestricted	32	static,ignoring_inline_asm

Debug/Middlewares/Third_Party/FreeRTOS/Source/timers.d

@@ -7,6 +7,7 @@ Middlewares/Third_Party/FreeRTOS/Source/timers.o: \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h \
+ ../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h \
  ../Middlewares/Third_Party/FreeRTOS/Source/include/queue.h \
@@ -19,6 +20,7 @@ Middlewares/Third_Party/FreeRTOS/Source/timers.o: \
 ../Middlewares/Third_Party/FreeRTOS/Source/include/deprecated_definitions.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/mpu_wrappers.h:
+../Middlewares/Third_Party/FreeRTOS/Source/include/newlib-freertos.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/task.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/list.h:
 ../Middlewares/Third_Party/FreeRTOS/Source/include/queue.h:

Trinity_Devboard_V2.ioc

@@ -76,15 +76,18 @@ Dma.SPI1_TX.0.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
 Dma.SPI1_TX.0.SyncRequestNumber=1
 Dma.SPI1_TX.0.SyncSignalID=NONE
 FREERTOS.FootprintOK=true
-FREERTOS.IPParameters=Tasks01,configMINIMAL_STACK_SIZE,FootprintOK,configTOTAL_HEAP_SIZE,Queues01
+FREERTOS.IPParameters=Tasks01,configMINIMAL_STACK_SIZE,FootprintOK,configTOTAL_HEAP_SIZE,Queues01,configCHECK_FOR_STACK_OVERFLOW,configUSE_NEWLIB_REENTRANT
 FREERTOS.Queues01=Q_ICM,1,ICM45686_Data,0,Dynamic,NULL,NULL;Q_BMM,1,struct bmm350_mag_temp_data,0,Dynamic,NULL,NULL
 FREERTOS.Tasks01=Task_DataAnalys,24,512,Start_DataAnalysis,Default,NULL,Dynamic,NULL,NULL;Task_ICM45686,24,512,Start_ICM45686,Default,NULL,Dynamic,NULL,NULL;Task_BMM350,24,512,Start_BMM350,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.configCHECK_FOR_STACK_OVERFLOW=2
 FREERTOS.configMINIMAL_STACK_SIZE=512
 FREERTOS.configTOTAL_HEAP_SIZE=64000
+FREERTOS.configUSE_NEWLIB_REENTRANT=1
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
-I2C1.IPParameters=Timing
+I2C1.I2C_Speed_Mode=I2C_Fast
-I2C1.Timing=0x10B17DB5
+I2C1.IPParameters=Timing,I2C_Speed_Mode
+I2C1.Timing=0x00C12166
 KeepUserPlacement=false
 MMTAppReg1.MEMORYMAP.AppRegionName=RAM
 MMTAppReg1.MEMORYMAP.ContextName=Cortex-M4NS
@@ -229,12 +232,17 @@ PA1.GPIOParameters=GPIO_Label
 PA1.GPIO_Label=GPIO_STATUS_LED
 PA1.Locked=true
 PA1.Signal=S_TIM2_CH2
-PA10.GPIOParameters=GPIO_Label
+PA10.GPIOParameters=GPIO_PuPd,GPIO_Label
 PA10.GPIO_Label=GPIO_IMU_INT
+PA10.GPIO_PuPd=GPIO_NOPULL
 PA10.Locked=true
 PA10.Signal=GPXTI10
+PA11.GPIOParameters=GPIO_Speed
+PA11.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
 PA11.Mode=Device
 PA11.Signal=USB_DM
+PA12.GPIOParameters=GPIO_Speed
+PA12.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
 PA12.Mode=Device
 PA12.Signal=USB_DP
 PA13.Mode=Trace_Asynchronous_SW
@@ -272,8 +280,14 @@ PA9.Mode=Full_Duplex_Master
 PA9.Signal=SPI2_SCK
 PB3.Mode=Trace_Asynchronous_SW
 PB3.Signal=SYS_JTDO-SWO
+PB8.GPIOParameters=GPIO_Speed,GPIO_FM8
+PB8.GPIO_FM8=SYSCFG_FASTMODEPLUS_PB8
+PB8.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
 PB8.Mode=I2C
 PB8.Signal=I2C1_SCL
+PB9.GPIOParameters=GPIO_Speed,GPIO_FM9
+PB9.GPIO_FM9=SYSCFG_FASTMODEPLUS_PB9
+PB9.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
 PB9.Mode=I2C
 PB9.Signal=I2C1_SDA
 PC1.GPIOParameters=GPIO_Speed,GPIO_Label
@@ -425,10 +439,10 @@ SH.S_TIM2_CH2.0=TIM2_CH2,PWM Generation2 CH2
 SH.S_TIM2_CH2.ConfNb=1
 SH.S_TIM2_CH3.0=TIM2_CH3,PWM Generation3 CH3
 SH.S_TIM2_CH3.ConfNb=1
-SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_64
+SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16
 SPI1.CLKPhase=SPI_PHASE_2EDGE
 SPI1.CLKPolarity=SPI_POLARITY_HIGH
-SPI1.CalculateBaudRate=1000.0 KBits/s
+SPI1.CalculateBaudRate=4.0 MBits/s
 SPI1.DataSize=SPI_DATASIZE_8BIT
 SPI1.Direction=SPI_DIRECTION_2LINES
 SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPhase,CLKPolarity
@@ -441,7 +455,8 @@ SPI2.Mode=SPI_MODE_MASTER
 SPI2.VirtualType=VM_MASTER
 STM32_WPAN.BT_SIG_HEART_RATE_SENSOR=Disabled
 STM32_WPAN.CFG_BONDING_MODE=0
-STM32_WPAN.IPParameters=BT_SIG_HEART_RATE_SENSOR,CFG_BONDING_MODE
+STM32_WPAN.CFG_LSCLK_LSE=0
+STM32_WPAN.IPParameters=BT_SIG_HEART_RATE_SENSOR,CFG_BONDING_MODE,CFG_LSCLK_LSE
 TIM2.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1
 TIM2.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2
 TIM2.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3

USB_Device/Target/usbd_conf.c

@@ -103,7 +103,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef* pcdHandle)
     GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_12;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
     GPIO_InitStruct.Pull = GPIO_NOPULL;
-    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
     GPIO_InitStruct.Alternate = GPIO_AF10_USB;
     HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);