get/set param for ICM bridge

Core/Inc/app_imu.h

@@ -28,10 +28,11 @@ nBus_sensorType_t mcu_spi_getType(uint8_t sensor_index);
 uint8_t mcu_spi_getSensorCount();
 uint8_t mcu_spi_getData(uint8_t sensor_index, uint8_t *data);
 uint8_t mcu_spi_setData(uint8_t *data);
-uint8_t mcu_spi_getParam(uint8_t sensor_index, nBus_param_t param);
+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, uint8_t value);
+nBus_param_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, int32_t value);
 void mcu_spi_readData(void);
+uint8_t mcu_spi_store(void);
 
 #ifdef __cplusplus
 }

Core/Src/app_imu.cpp

@@ -24,7 +24,8 @@ nBusAppInterface_t mcu_spi_driver = {
 	  mcu_spi_setParam,
 	  mcu_spi_start,
 	  mcu_spi_stop,
-	  mcu_spi_readData
+	  mcu_spi_readData,
+	  mcu_spi_store,
 };
 
 
@@ -37,6 +38,109 @@ nBusAppInterface_t *getImuDriver(){
 	return &mcu_spi_driver;
 }
 
+static accel_samplerate getAccelParamSamplerate(int32_t value){
+
+	switch(value){
+	case ACCEL_samplerate_0_27Hz:
+	case ACCEL_samplerate_0_55Hz:
+	case ACCEL_samplerate_102_3Hz:
+	case ACCEL_samplerate_140_6Hz:
+	case ACCEL_samplerate_17_6Hz:
+	case ACCEL_samplerate_187_5Hz:
+	case ACCEL_samplerate_1_1Hz:
+	case ACCEL_samplerate_281_3Hz:
+	case ACCEL_samplerate_2_2Hz:
+	case ACCEL_samplerate_35_2Hz:
+	case ACCEL_samplerate_48_9Hz:
+	case ACCEL_samplerate_4_4Hz:
+	case ACCEL_samplerate_562_5Hz:
+	case ACCEL_samplerate_70_3Hz:
+	case ACCEL_samplerate_8_8Hz:
+		return (accel_samplerate)value;
+	}
+	return ACCEL_samplerate_None;
+
+}
+
+
+static accel_full_scale getAccelParamFullScale(int32_t value){
+	switch(value){
+	case ACCEL_FS_2g:
+	case ACCEL_FS_4g:
+	case ACCEL_FS_8g:
+	case ACCEL_FS_16g:
+		return (accel_full_scale)value;
+	}
+	return ACCEL_FS_NoneG;
+}
+
+
+static accel_dlp_cfg getAccelParamLowPassFilter(int32_t value){
+	switch(value){
+	case ACCEL_lpf_005_7Hz:
+	case ACCEL_lpf_011_5Hz:
+	case ACCEL_lpf_023_9Hz:
+	case ACCEL_lpf_050_4Hz:
+	case ACCEL_lpf_114_4Hz:
+	case ACCEL_lpf_246Hz:
+	case ACCEL_lpf_473Hz:
+	case ACCEL_lpf_OFF:
+		return (accel_dlp_cfg)value;
+	}
+	return ACCEL_lpf_None;
+}
+
+static gyro_samplerate getGyroParamSamplerate(int32_t value){
+
+	switch(value){
+	case GYRO_samplerate_004_4Hz:
+	case GYRO_samplerate_017_3Hz:
+	case GYRO_samplerate_017_6Hz:
+	case GYRO_samplerate_034_1Hz:
+	case GYRO_samplerate_035_2Hz:
+	case GYRO_samplerate_048_9Hz:
+	case GYRO_samplerate_066_2Hz:
+	case GYRO_samplerate_070_3Hz:
+	case GYRO_samplerate_102_3Hz:
+	case GYRO_samplerate_125_0Hz:
+	case GYRO_samplerate_140_6Hz:
+	case GYRO_samplerate_187_5Hz:
+	case GYRO_samplerate_225_0Hz:
+	case GYRO_samplerate_281_3Hz:
+	case GYRO_samplerate_375_0Hz:
+	case GYRO_samplerate_562_5Hz:
+		return (gyro_samplerate)value;
+	}
+	return GYRO_samplerate_None;
+}
+
+static gyro_full_scale getGyroParamFullScale(int32_t value){
+	switch(value){
+	case GYRO_FS_1000dps:
+	case GYRO_FS_2000dps:
+	case GYRO_FS_250dps:
+	case GYRO_FS_500dps:
+		return (gyro_full_scale)value;
+	}
+	return GYRO_FS_None;
+}
+
+static gyro_dlp_cfg getGyroParamLowPassFilter(int32_t value){
+	switch(value){
+	case GYRO_low_pass_OFF:
+	case GYRO_lpf_005_7Hz:
+	case GYRO_lpf_011_6Hz:
+	case GYRO_lpf_023_9Hz:
+	case GYRO_lpf_051_2Hz:
+	case GYRO_lpf_119_5Hz:
+	case GYRO_lpf_151_8Hz:
+	case GYRO_lpf_196_6Hz:
+	case GYRO_lpf_361_4Hz:
+		return (gyro_dlp_cfg)value;
+	}
+	return GYRO_lpf_None;
+}
+
 void mcu_spi_init(void *hw_interface, void *hw_config){
 	manager = new IcmSpiManager((SPI_HandleTypeDef*)hw_interface);		// TODO toto ma byt o uroven vyssie, ale je to c subor
 	sensor = new Icm20948(manager, (icm20948_Config*)hw_config);
@@ -99,17 +203,156 @@ uint8_t mcu_spi_getData(uint8_t sensor_index, uint8_t *data){
 }
 
 uint8_t mcu_spi_setData(uint8_t *data){
-	return 0;
+	return 1; // ILLEGAL_FUNCTION;
 }
 
-uint8_t mcu_spi_getParam(uint8_t sensor_index, nBus_param_t param){
-	return 0x00;
+int32_t mcu_spi_getParam(uint8_t sensor_index, nBus_param_t param){
+	uint32_t param_value = PARAM_VALUE_NONE;
+	// to module
+	if(sensor_index == 0) {
+		param_value = PARAM_VALUE_NONE;
+	}
+
+	if(sensor_index == 1) {
+		switch(param){
+		case PARAM_SAMPLERATE:
+			param_value = sensor->accSensor->GetSampleRate();
+			break;
+		case PARAM_RANGE:
+			param_value = sensor->accSensor->GetRange();
+			break;
+		case PARAM_FILTER:
+			param_value = sensor->accSensor->GetLowPassFilter();
+			break;
+		default:
+			param_value = PARAM_VALUE_NONE;
+		}
+	}
+
+	if(sensor_index == 2) {
+		switch(param){
+		case PARAM_SAMPLERATE:
+			param_value = sensor->gyroSensor->GetSampleRate();
+			break;
+		case PARAM_RANGE:
+			param_value = sensor->gyroSensor->GetRange();
+			break;
+		case PARAM_FILTER:
+			param_value = sensor->gyroSensor->GetLowPassFilter();
+			break;
+		default:
+			param_value = PARAM_VALUE_NONE;
+		}
+	}
+
+	return param_value;
 }
 
 uint8_t mcu_spi_hasParam(uint8_t sensor_index, nBus_param_t param){
+	if(sensor_index == 1 || sensor_index == 2){
+		switch(param){
+		case PARAM_SAMPLERATE:
+		case PARAM_RANGE:
+		case PARAM_FILTER:
+			return 1;
+		default:
+			return 0;
+		}
+		return 0;
+	}
 	return 0;
 }
 
-nBus_param_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, uint8_t value){
-	return PARAM_NONE;
+nBus_param_t mcu_spi_setParam(uint8_t sensor_index, nBus_param_t param, int32_t value) {
+	// to module
+	if(sensor_index == 0) {
+		return PARAM_NONE;
+	}
+
+	if(sensor_index == 1) {
+		switch(param){
+		case PARAM_SAMPLERATE:
+		{
+			accel_samplerate sr = getAccelParamSamplerate(value);
+			if(sr != ACCEL_samplerate_None) {
+				sensor->accSensor->SetSampleRate(sr);
+			} else {
+				return PARAM_NONE;
+			}
+
+		}
+			break;
+		case PARAM_RANGE:
+		{
+			accel_full_scale fs = getAccelParamFullScale(value);
+			if(fs != ACCEL_FS_NoneG) {
+				sensor->accSensor->SetRange(fs);
+			} else {
+				return PARAM_NONE;
+			}
+
+		}
+			break;
+		case PARAM_FILTER:
+		{
+			accel_dlp_cfg lpf = getAccelParamLowPassFilter(value);
+			if(lpf != ACCEL_lpf_None) {
+				sensor->accSensor->SetLowPassFilter(lpf);
+			} else {
+				return PARAM_NONE;
+			}
+
+		}
+			break;
+		default:
+			return PARAM_NONE;
+		}
+	}
+
+	if(sensor_index == 2) {
+		switch(param){
+		case PARAM_SAMPLERATE:
+		{
+			gyro_samplerate sr = getGyroParamSamplerate(value);
+			if(sr != GYRO_samplerate_None) {
+				sensor->gyroSensor->SetSampleRate(sr);
+			} else {
+				return PARAM_NONE;
+			}
+
+		}
+			break;
+		case PARAM_RANGE:
+		{
+			gyro_full_scale fs = getGyroParamFullScale(value);
+			if(fs != GYRO_FS_None) {
+				sensor->gyroSensor->SetRange(fs);
+			} else {
+				return PARAM_NONE;
+			}
+
+		}
+			break;
+		case PARAM_FILTER:
+		{
+			gyro_dlp_cfg lpf = getGyroParamLowPassFilter(value);
+			if(lpf != GYRO_lpf_None) {
+				sensor->gyroSensor->SetLowPassFilter(lpf);
+			} else {
+				return PARAM_NONE;
+			}
+
+		}
+			break;
+		default:
+			return PARAM_NONE;
+		}
+	}
+
+	return param;
+
+}
+
+uint8_t mcu_spi_store(void){
+	return 0;
 }

Modules/icm20948

@@ -1 +1 @@
-Subproject commit 15ae086226c61ea917c815ac37be0e23511999c2
+Subproject commit afabaea4a749ab11a4d11d0c4fde1ea4df257af5

Modules/nbus

@@ -1 +1 @@
-Subproject commit 143c92dd34a60ef87476b04d8221a695b56f2407
+Subproject commit 5e72d669952141f67c40a36ad9f4b2be840886fd

baModule-slave Debug.launch

@@ -37,7 +37,7 @@
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.max_halt_delay" value="2"/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.reset_strategy" value="connect_under_reset"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_check_serial_number" value="false"/>
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value="066CFF555775514867081543"/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>

test/app.py

@@ -24,11 +24,28 @@ class AppTest:
             echo = echo + chr(resp[3+r])
         print("Echo:"+echo)
 
-    def cmd_set_param(self, param):        
-        resp = self.serial_port.request(self.module, self.srensor, (SET+CMD_PARAM),[])     
+    def cmd_set_param(self, sensor, param, value):                
+        print("SET param:", PARAM_NAME[param], "[", param, "]=>", value)
+        int_to_four_bytes = struct.Struct('<I').pack
+        y1, y2, y3, y4 = int_to_four_bytes(value & 0xFFFFFFFF)
+        resp = self.serial_port.request(self.module, sensor, (SET+CMD_PARAM), [param, y1, y2, y3, y4])     
+
+    def cmd_get_param_module(self, param):   
+        print("GET module param:", PARAM_NAME[param], "[", param, "]")
+        resp = self.serial_port.request(self.module, 0, (CMD_PARAM),[param])     
+
+    def cmd_get_param(self, sensor, param):        
+        print("GET param:", PARAM_NAME[param], "[", param, "]")
+        resp = self.serial_port.request(self.module, sensor, (CMD_PARAM),[param])    
+        val = None
+        
+        if len(resp) > 2:
+            val = resp[4] + resp[5]*256 + resp[6]*256*256 + resp[7] * 256*256*256
+        return val
 
-    def cmd_get_param(self, param):        
-        resp = self.serial_port.request(self.module, self.sensor, (CMD_PARAM),[])     
+    def cmd_get_params(self, sensor):
+        print("GET params:")
+        resp = self.serial_port.request(self.module, sensor, (CMD_PARAM),[])    
 
     def cmd_sensor_cnt(self):
         print("SENSOR CNT")
@@ -120,9 +137,21 @@ if __name__ == "__main__":
     #app.cmd_version()
     #app.cmd_version()
     # app.cmd_module_stop()
-    app.cmd_echo([65,66,67,68,69,70])
+    app.cmd_echo([65])
     app.cmd_echo([97,98,99,100])
-    #sys.exit()
+
+    app.cmd_set_param(1, PARAM_SAMPLERATE, 10)
+    app.cmd_set_param(1, PARAM_RANGE, 1)
+    app.cmd_set_param(1, PARAM_FILTER, 2)
+    
+    for s in range(1):
+        sr=app.cmd_get_param(s+1,PARAM_SAMPLERATE)
+        r=app.cmd_get_param(s+1,PARAM_RANGE)
+        lpf=app.cmd_get_param(s+1,PARAM_FILTER)
+        ee=app.cmd_get_param(s+1,PARAM_GAIN)
+        print(sr,r,lpf,ee)
+    
+    sys.exit()
     #time.sleep(0.5)
     #app.cmd_module_stop()
     time.sleep(0.5)

test/comm.py

@@ -26,6 +26,28 @@ CMD_SYNC = 0x0C
 CMD_SENSOR_TYPE = 0x0D
 CMD_INFO = 0x0E
 
+PARAM_NONE = 0xFF
+PARAM_TIMEBASE = 0
+PARAM_RESOLUTION = 1
+PARAM_GAIN = 2 
+PARAM_OFFSET = 3
+PARAM_SAMPLERATE = 4
+PARAM_RANGE = 5
+PARAM_RANGE0 = 6
+PARAM_FILTER = 7
+
+PARAM_NAME = {
+    PARAM_NONE : "NONE",
+    PARAM_TIMEBASE : "TIMEBASE",
+    PARAM_RESOLUTION : "RESOLUTION",
+    PARAM_GAIN : "GAIN",
+    PARAM_OFFSET : "OFFSET",
+    PARAM_SAMPLERATE : "SAMPLERATE",
+    PARAM_RANGE : "RANGE",
+    PARAM_RANGE0 : "RANGE0",
+    PARAM_FILTER : "FILTER",
+}
+
 crc8x_table = [
         0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
         0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
@@ -62,6 +84,10 @@ def crc8(data):
 
 def local_logger(*message):
     for i in range(len(message)):
+        if (type(message[i]) == type([])):
+            for m in message[i]:
+                print(hex(m), end="|")
+            continue
         print(message[i], end=" ")
     print()
 
@@ -134,7 +160,7 @@ class SerialComm:
         response = self.port.read(ord(response_l))
         for bajt in response:
             data.append(bajt)
-        self.callback('d', 0, "\tRS=>", data)
+        self.callback('d', 0, "\tRS>", data)
         return data
 
     def requestBroadcast(self, command, data):
@@ -161,7 +187,7 @@ class SerialComm:
             if counter > 5:
                 return []
         if data[2] & 0x80 != 0:
-            print("Chyba", data[3])
+            print("Chyba", hex(data[3]))
             return [data[3]]
         return data