Merge branch 'multislave' into 'main'

uprava callbackov

See merge request sensorical/fw/ban-module!3

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

@@ -109,6 +109,10 @@ inline void uart_send(uint8_t *data, int n)
 inline void uart_receive(uint8_t *dataNBUS, int n)
 {
 	dataUART = dataNBUS;
+	dataL = 0;
+	dataI = 0;
+	data[0] = 0;
+	uart_timeout = HAL_GetTick();
 	HAL_UARTEx_ReceiveToIdle_DMA(&huart2, data, n);
 }
 
@@ -128,24 +132,30 @@ inline void app_delay(uint8_t ms){
 	HAL_Delay(ms);
 }
 
-static inline uint8_t loop_callback() {
-
+static inline uint8_t loop_callback(nBusStateCallbackType_t state_check) {
+// traba kontrolovat kazdu stav (state_check) zvmlast. Moznost doplnit dalsie kontroly
+	if (state_check == CallbackType_SENSOR) {
 #if MODULE == MODULE_IMU
-	if(icm_data_ready == 1){
-		icm_data_ready = 0;
-		return 1;  // interrupt from external sensor: data ready
-	}
+		if(icm_data_ready == 1){
+			icm_data_ready = 0;
+			return 1;  // interrupt from external sensor: data ready
+		}
 #endif
+		return 0;
+	}
 
-	// timemout: 1 - 2 ms
-	if(HAL_GetTick() - 1 > uart_timeout) {
-		dataL = 0;
-		dataI = 0;
-		data[0] = 0;
-		uart_timeout = MAX_SYSTICK;
-		led_off();
-		HAL_UARTEx_ReceiveToIdle_DMA(&huart2, data, 64);
-		return 2;  // UART receive timeout
+	if (state_check == CallbackType_UART) {
+		// timemout: 1 - 2 ms
+		if(HAL_GetTick() - 1 > uart_timeout) {
+			dataL = 0;
+			dataI = 0;
+			data[0] = 0;
+			uart_timeout = MAX_SYSTICK;
+			led_off();
+			HAL_UARTEx_ReceiveToIdle_DMA(&huart2, data, 64);
+			return 1;  // UART receive timeout
+		}
+		return 0;
 	}
 
 	return 0;
@@ -198,6 +208,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
   */
 int main(void)
 {
+
   /* USER CODE BEGIN 1 */
 
   /* USER CODE END 1 */
@@ -225,7 +236,6 @@ int main(void)
   MX_TIM21_Init();
   MX_SPI1_Init();
   /* USER CODE BEGIN 2 */
-
   dataI=0;
   dataL=0;
 

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 899d201b1559ccfc79e1697a116cfff18c74856b

Modules/nbus

@@ -1 +1 @@
-Subproject commit c8ce08de5dfdfac620d81952868f2cbcff5abf18
+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

@@ -19,15 +19,28 @@ if __name__ == "__main__":
     s1_adr = 5
     s2_adr = 6
     adrs = [s1_adr, s2_adr]
-    slave1 = nbus.create_slave(ImuSlave, s1_adr)
-    slave2 = nbus.create_slave(ImuSlave, s2_adr)
+    # adrs = [s1_adr]
 
-    if nbus.get_slave(s1_adr).cmd_echo([97, 98, 99, 100]) == 0:
-        print("Slave 1 not found")
-        sys.exit()
+    foundSlaves = {}
+    for adr in adrs:
+        slave1 = nbus.create_slave(ImuSlave, adr)
+        foundSlaves[adr] = True
 
-    if nbus.get_slave(s2_adr).cmd_echo([97, 98, 99, 100]) == 0:
-        print("Slave 2 not found")
+        # 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:
+            foundSlaves[adr] = False
+    for s in foundSlaves:
+        startCheck = startCheck and foundSlaves[s]
+
+    print("Module presence:", foundSlaves)
+
+    if not startCheck:
         sys.exit()
 
     # app.cmd_reset()
@@ -51,15 +64,10 @@ if __name__ == "__main__":
             ee = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_GAIN)
             print(sr, r, lpf, ee)
 
-        sr = nbus.get_slave(adr).cmd_get_param(1, PARAM_SAMPLERATE)
-
-        nbus.get_slave(adr).cmd_set_param(1, PARAM_RANGE, 1)
-        print(nbus.get_slave(adr).slave_params)
+        # sr = nbus.get_slave(adr).cmd_get_param(1, PARAM_SAMPLERATE)
 
-    nbus.cmd_start()
-    time.sleep(0.1)
-    #nbus.cmd_stop()
-    #sys.exit()
+        # nbus.get_slave(adr).cmd_set_param(1, PARAM_RANGE, 1)
+        # print(nbus.get_slave(adr).slave_params)
 
     pocet = {}
     for adr in adrs:
@@ -67,6 +75,9 @@ if __name__ == "__main__":
         # sys.exit()
         print("pocet senzorov=", pocet)
 
+    nbus.cmd_start()
+    time.sleep(0.1)
+    #nbus.cmd_stop()
     #sys.exit()
 
     data_acc = {}
@@ -78,8 +89,8 @@ 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(gyr)
-            print(".", end=" ")
+            # print(acc)
+        print(".", end=" ")
     nbus.cmd_stop()
     show_data(data_acc)
 

test/comm.py

@@ -155,6 +155,7 @@ class SerialComm:
 
     def receive_response(self):
         data = []
+        # time.sleep(0.005)
         response_l = self.port.read(1)
 
         if len(response_l) == 0:

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]