FC=calibrate

Core/Inc/app_imu.h

@@ -32,6 +32,15 @@ int32_t mcu_spi_getParam(uint8_t sensor_index, nBus_param_t param);
 uint8_t mcu_spi_hasParam(uint8_t sensor_index, nBus_param_t param);
 nBus_param_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, int32_t value);
 void mcu_spi_readData(void);
+/**
+ * Run calibration procedure.
+ * @param subslaveIndex index of sublave (specific sensor) to run calibration procedure. Can be: 0 - all sensors, 1, 2 - sensor index
+ * @param calibrationParamsNum number of input calibration values for sensors
+ * @param calibrationParams array of calibration parameters
+ * @note calibrationParams  needs to be updated, because it is only uint8_t array for now
+ * @return 1 - success, 0 - sensor not found (when subslaveIndex != 0)
+ */
+uint8_t mcu_spi_calibrate(uint8_t subslaveIndex, uint8_t calibrationParamsNum, uint8_t *calibrationParams);
 //uint8_t mcu_spi_store(void);
 
 #ifdef __cplusplus

Core/Src/app_imu.cpp

@@ -25,7 +25,8 @@ nBusAppInterface_t mcu_spi_driver = {
 	  mcu_spi_start,
 	  mcu_spi_stop,
 	  mcu_spi_readData,
-	  //mcu_spi_store,
+	  NULL, //mcu_spi_store,
+	  mcu_spi_calibrate,
 };
 
 
@@ -164,14 +165,17 @@ void mcu_spi_readData(void){
 
 nBus_sensorType_t mcu_spi_getType(uint8_t sensor_index){
 
-	if (sensor_index > mcu_spi_getSensorCount())
+	if (sensor_index > mcu_spi_getSensorCount()) {
 		return TYPE_UNKNOWN;
+	}
 
-	if (sensor_index == 1)
+	if (sensor_index == 1) {
 		return TYPE_ACCELEROMETER;
+	}
 
-	if (sensor_index == 2)
+	if (sensor_index == 2) {
 		return TYPE_GYROSCOPE;
+	}
 
 	return TYPE_UNKNOWN;
 }
@@ -352,6 +356,43 @@ nBus_param_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, int32_t
 	return param;
 
 }
+
+/**
+ * @param sebslaveIndex sensorAccIndex, sensorGyroIndex, sensorAll
+ */
+uint8_t mcu_spi_calibrate(uint8_t subslaveIndex, uint8_t calibrationParamsNum, uint8_t *calibrationParams){
+
+	uint8_t wasReady = sensor->IsReady();
+	sensor->Stop();
+	uint8_t success = 0;
+	if (subslaveIndex == (uint8_t) sensorAccIndex){
+		sensor->accSensor->Calibrate();
+		success = 1;
+	}
+
+	if (subslaveIndex == (uint8_t) sensorGyroIndex){
+		sensor->gyroSensor->Calibrate();
+		success = 1;
+	}
+
+//	if (subslaveIndex == sensorMagIndex){
+//		sensor->magSensor->Calibrate();
+//	}
+
+	if (subslaveIndex == (uint8_t) sensorAll){
+		sensor->accSensor->Calibrate();
+		sensor->gyroSensor->Calibrate();
+//		sensor->magSensor->Calibrate();
+		success = 1;
+	}
+
+	if (wasReady !=0 ) {
+		sensor->Start();
+	}
+
+	return success;
+}
+
 /*
 uint8_t mcu_spi_store(void){
 	return 0;

Core/Src/main.c

@@ -208,6 +208,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
   */
 int main(void)
 {
+
   /* USER CODE BEGIN 1 */
 
   /* USER CODE END 1 */

Core/Src/stm32l0xx_hal_msp.c

@@ -20,7 +20,6 @@
 
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
-
 /* USER CODE BEGIN Includes */
 
 /* USER CODE END Includes */
@@ -68,6 +67,7 @@ extern DMA_HandleTypeDef hdma_usart2_rx;
   */
 void HAL_MspInit(void)
 {
+
   /* USER CODE BEGIN MspInit 0 */
 
   /* USER CODE END MspInit 0 */

Modules/icm20948

@@ -1 +1 @@
-Subproject commit acb82f70b604b3e4adbcf0015d02f7536cb21b9c
+Subproject commit 6a42900cd1721ef7008690af7f01b7e0b76809c8

Modules/nbus

@@ -1 +1 @@
-Subproject commit 1cb4aa770fc42b13afe4750d17513c1d0c0d133c
+Subproject commit 1e1195a2f4310ea39b5999626e9a7d0cb0e948d7

baModule-slave.ioc

@@ -84,8 +84,8 @@ Mcu.PinsNb=26
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32L031K6Tx
-MxCube.Version=6.10.0
-MxDb.Version=DB.6.0.100
+MxCube.Version=6.11.1
+MxDb.Version=DB.6.0.111
 NVIC.ADC1_COMP_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true
 NVIC.DMA1_Channel1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
 NVIC.DMA1_Channel4_5_6_7_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true
@@ -211,7 +211,7 @@ ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=true
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_USART2_UART_Init-USART2-false-HAL-true,5-MX_TIM22_Init-TIM22-false-HAL-true,6-MX_TIM21_Init-TIM21-false-HAL-true,7-MX_ADC_Init-ADC-true-HAL-true,8-MX_SPI1_Init-SPI1-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_USART2_UART_Init-USART2-false-HAL-true,5-MX_TIM21_Init-TIM21-false-HAL-true,6-MX_ADC_Init-ADC-true-HAL-true,7-MX_SPI1_Init-SPI1-false-HAL-true
 RCC.48CLKFreq_Value=24000000
 RCC.AHBFreq_Value=32000000
 RCC.APB1Freq_Value=32000000

test/app.py

@@ -18,15 +18,19 @@ if __name__ == "__main__":
     nbus = NbusSystem(False)
     s1_adr = 5
     s2_adr = 6
-    # adrs = [s1_adr, s2_adr]
-    adrs = [s1_adr]
+    adrs = [s1_adr, s2_adr]
+    # adrs = [s1_adr]
 
     foundSlaves = {}
     for adr in adrs:
         slave1 = nbus.create_slave(ImuSlave, adr)
         foundSlaves[adr] = True
-    # slave2 = nbus.create_slave(ImuSlave, s2_adr)
 
+        # r = nbus.get_slave(adr).cmd_calibrate(0)
+        # print(r)
+
+    # slave2 = nbus.create_slave(ImuSlave, s2_adr)
+    # sys.exit(0)
     startCheck = True
     for adr in adrs:
         if nbus.get_slave(adr).cmd_echo([97, 98, 99, 100]) == 0:
@@ -85,7 +89,7 @@ if __name__ == "__main__":
             acc = nbus.get_slave(adr).cmd_sensor_get_data_IMU(1)
             # gyr = nbus.get_slave(adr).cmd_sensor_get_data_IMU(2)
             data_acc[adr].append(acc)
-            print(acc)
+            # print(acc)
         print(".", end=" ")
     nbus.cmd_stop()
     show_data(data_acc)

test/nbus_slave.py

@@ -134,3 +134,10 @@ class NbusSlave():
     def cmd_store(self, sensor):
         print("MODULE STORE PARAM")
         resp = self.serial_port.request(self.module, sensor, (SET + CMD_STORE), [], long_answer=0.1)
+
+    def cmd_calibrate(self, sensor):
+        print(f"MODULE [{self.module}/{sensor}] RUN CALIBRATION")
+        resp = self.serial_port.request(self.module, sensor, (SET + CMD_CALIBRATE), [], long_answer=3 if sensor == 0 else 1.5)
+        if len(resp) == 1:
+            return 0
+        return resp[3]