- 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

Debug/Core/Src/sensors.cyclo

@@ -1,17 +1,17 @@
-../Core/Src/sensors.c:18:6:HAL_SPI_TxRxCpltCallback	4
-../Core/Src/sensors.c:27:6:HAL_I2C_MemRxCpltCallback	4
-../Core/Src/sensors.c:36:6:HAL_I2C_MemTxCpltCallback	4
-../Core/Src/sensors.c:45:6:HAL_I2C_ErrorCallback	4
-../Core/Src/sensors.c:55:6:HAL_GPIO_EXTI_Callback	6
-../Core/Src/sensors.c:70:19:read_icm	1
-../Core/Src/sensors.c:77:19:read_icm_dma	1
-../Core/Src/sensors.c:83:19:write_icm	1
-../Core/Src/sensors.c:90:6:init_icm	8
-../Core/Src/sensors.c:160:6:calibrate_icm	4
-../Core/Src/sensors.c:228:6:configure_and_enable_int_icm	7
-../Core/Src/sensors.c:284:6:bmm_init_DWT	1
-../Core/Src/sensors.c:295:22:bmm350_i2c_read	3
-../Core/Src/sensors.c:327:22:bmm350_i2c_write	3
-../Core/Src/sensors.c:355:6:bmm350_delay	2
-../Core/Src/sensors.c:366:8:init_bmm	2
-../Core/Src/sensors.c:405:8:bmm_configure_interrupt	1
+../Core/Src/sensors.c:15:6:HAL_SPI_TxRxCpltCallback	4
+../Core/Src/sensors.c:24:6:HAL_I2C_MemRxCpltCallback	4
+../Core/Src/sensors.c:33:6:HAL_I2C_MemTxCpltCallback	4
+../Core/Src/sensors.c:41:6:HAL_I2C_ErrorCallback	4
+../Core/Src/sensors.c:50:6:HAL_GPIO_EXTI_Callback	6
+../Core/Src/sensors.c:62:19:read_icm	1
+../Core/Src/sensors.c:69:19:read_icm_dma	1
+../Core/Src/sensors.c:75:19:write_icm	1
+../Core/Src/sensors.c:82:6:init_icm	8
+../Core/Src/sensors.c:152:6:calibrate_icm	4
+../Core/Src/sensors.c:204:6:configure_and_enable_int_icm	7
+../Core/Src/sensors.c:260:6:bmm_init_DWT	1
+../Core/Src/sensors.c:269:22:bmm350_i2c_read	3
+../Core/Src/sensors.c:282:22:bmm350_i2c_write	3
+../Core/Src/sensors.c:292:6:bmm350_delay	2
+../Core/Src/sensors.c:302:8:init_bmm	2
+../Core/Src/sensors.c:333:8:bmm_configure_interrupt	1