update app PY

Core/Src/app_imu.cpp

@@ -210,7 +210,7 @@ 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;
+		return PARAM_VALUE_NONE;
 	}
 
 	if(sensor_index == 1) {

test/.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Editor-based HTTP Client requests
+/httpRequests/
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml

test/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

test/.idea/misc.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="Black">
+    <option name="sdkName" value="Python 3.10" />
+  </component>
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.10" project-jdk-type="Python SDK" />
+</project>

test/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/test.iml" filepath="$PROJECT_DIR$/.idea/test.iml" />
+    </modules>
+  </component>
+</project>

test/.idea/test.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

test/.idea/vcs.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+    <mapping directory="$PROJECT_DIR$/../Modules/dataframe" vcs="Git" />
+    <mapping directory="$PROJECT_DIR$/../Modules/icm20948" vcs="Git" />
+    <mapping directory="$PROJECT_DIR$/../Modules/nbus" vcs="Git" />
+    <mapping directory="$PROJECT_DIR$/../Modules/one-wire-memory" vcs="Git" />
+  </component>
+</project>

test/app.py

@@ -3,81 +3,128 @@ import sys
 import time
 from struct import *
 
-class AppTest:
-    def __init__(self, adr_module, adr_sensor):
-        self.serial_port = SerialComm('/dev/ttyUSB0')
-        self.module = adr_module
-        self.sensor = adr_sensor
+
+class NbusSystem:
+    slaves = []
+
+    def __init__(self, debug = False):
+        self.serial_port = SerialComm('/dev/ttyUSB0', debug)
+
+    def create_slave(self, slave):
+        slave = NbusSlave(slave, self.serial_port)
+        return slave
 
     def finish(self):
         self.serial_port.close()
 
-    def cmd_version(self):
-        resp = self.serial_port.request(self.module, 0, CMD_VERSION,[])
-        version = chr(resp[3])+chr(resp[4])+chr(resp[5])
-        print("Version: "+version)
 
-    def cmd_echo(self, msg):        
-        resp = self.serial_port.request(self.module, 0, CMD_ECHO,msg)        
+class NbusSlave:
+
+    def __init__(self, module, comm_port):
+        self.serial_port = comm_port
+        self.module = module
+
+    # def cmd_version(self):
+    #     resp = self.serial_port.request(self.module, 0, CMD_VERSION,[])
+    #     version = chr(resp[3])+chr(resp[4])+chr(resp[5])
+    #     print("Version: "+version)
+
+    def cmd_echo(self, msg):
+        resp = self.serial_port.request(self.module, 0, CMD_ECHO, msg)
         echo = ""
         if len(resp) == 0:
             print("No ECHO (0-size resp)")
             return 0
-        print(resp)
+        # print(resp)
         for r in range(len(msg)):
-            echo = echo + chr(resp[3+r])
-        print("Echo:"+echo)
+            echo = echo + chr(resp[3 + r])
+        # print("Echo:" + echo)
         return len(resp)
 
-    def cmd_set_param(self, sensor, param, value):                
+    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])     
+        resp = self.serial_port.request(self.module, sensor, (SET + CMD_PARAM), [param, y1, y2, y3, y4])
 
-    def cmd_get_param_module(self, param):   
+    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])     
+        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])    
+    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
+            val = resp[4] + resp[5] * 256 + resp[6] * 256 * 256 + resp[7] * 256 * 256 * 256
+        print(val)
+        value = -1
+        if param == PARAM_RANGE:
+            value = (val+1)**2    # +/- 2, 4, 8, 16g
+        if param == PARAM_SAMPLERATE:
+            if val == 1:
+                value = 562.5
+            if val == 3:
+                value = 281.3
+            if val == 5:
+                value = 187.5
+            if val == 7:
+                value = 140.6
+            if val == 10:
+                value = 102.3
+            if val == 15:
+                value = 70.3
+            if val == 22:
+                value = 48.9
+            if val == 31:
+                value = 32.2
+            if val == 63:
+                value = 17.6
+            if val == 127:
+                value = 8.8
+            if val == 255:
+                value = 4.4
+            if val == 513:
+                value = 2.2
+            if val == 1022:
+                value = 1.1
+            if val == 2044:
+                value = 0.55
+            if val == 4095:
+                value = 0.27
+
+        if param == PARAM_FILTER:
+            value = (val-1)
+        return value
 
     def cmd_get_params(self, sensor):
         print("GET params:")
-        resp = self.serial_port.request(self.module, sensor, (CMD_PARAM),[])    
+        resp = self.serial_port.request(self.module, sensor, (CMD_PARAM), [])
+        return resp
 
     def cmd_sensor_cnt(self):
         print("SENSOR CNT")
-        resp = self.serial_port.request(self.module, 0, CMD_SENSOR_CNT,[])        
+        resp = self.serial_port.request(self.module, 0, CMD_SENSOR_CNT, [])
         return resp[3]
 
     def cmd_sensor_type(self, index):
-        resp = self.serial_port.request(self.module, index, CMD_SENSOR_TYPE,[])
+        resp = self.serial_port.request(self.module, index, CMD_SENSOR_TYPE, [])
         return resp[3]
 
     def cmd_sensors_type(self, pocet):
-        resp = self.serial_port.request(self.module, 0, CMD_SENSOR_TYPE,[])
+        resp = self.serial_port.request(self.module, 0, CMD_SENSOR_TYPE, [])
         typy = [];
         for i in range(pocet):
-            typy.append([resp[3+2*i],resp[4+2*i]])
+            typy.append([resp[3 + 2 * i], resp[4 + 2 * i]])
         return typy
 
-    def cmd_sensor_get_params(self, index):
-        resp = self.serial_port.request(self.module, index, CMD_PARAM,[])
-        return resp
-
     def cmd_sensor_get_data(self, index):
-        resp = self.serial_port.request(self.module, index, CMD_DATA,[])
-        if len(resp)>1:
+        resp = self.serial_port.request(self.module, index, CMD_DATA, [])
+        if len(resp) > 1:
             h = resp[5] * 256 + resp[4]
-        # print(hex(h))        
-            return h/4096*3.3
+            # print(hex(h))
+            return h / 4096 * 3.3
         return 0
 
     def cmd_sensor_get_data_IMU(self, sensor_index):
@@ -86,151 +133,145 @@ class AppTest:
         1 - ACC
         2 - GYRO
         '''
-        resp = self.serial_port.request(self.module, sensor_index, CMD_DATA,[])
-        #print(resp)
+        resp = self.serial_port.request(self.module, sensor_index, CMD_DATA, [])
+        # print(resp)
         x = 0
         y = 0
         z = 0
-        if len(resp)>6:
-            x = resp[5]*256 + resp[4]
-            y = resp[7]*256 + resp[6]
-            z = resp[9]*256 + resp[8]
+        if len(resp) > 6:
+            x = resp[5] * 256 + resp[4]
+            y = resp[7] * 256 + resp[6]
+            z = resp[9] * 256 + resp[8]
             if x > 32768:
                 x = x - 65535
             if y > 32768:
-                y = y - 65535                
+                y = y - 65535
             if z > 32768:
-                z = z - 65535                
-        return [x,y,z]
+                z = z - 65535
+        return [x, y, z]
 
     def cmd_sensor_get_data_FSR(self, cnt):
-        resp = self.serial_port.request(self.module, 0, CMD_DATA,[])
+        resp = self.serial_port.request(self.module, 0, CMD_DATA, [])
         print(resp)
-        if len(resp)>1:
+        if len(resp) > 1:
             for i in range(cnt):
-                sen = resp[3+3*i]
-                h = resp[5+3*i] * 256 + resp[4+3*i]
-                print("FSR",sen,":\t", h/4096.0*3.3)
-        
-        
+                sen = resp[3 + 3 * i]
+                h = resp[5 + 3 * i] * 256 + resp[4 + 3 * i]
+                print("FSR", sen, ":\t", h / 4096.0 * 3.3)
 
     def cmd_module_info(self, param):
-        resp = self.serial_port.request(self.module, 0, CMD_INFO,[param])
+        resp = self.serial_port.request(self.module, 0, CMD_INFO, [param])
         if param == 0xE3:
-            data=0
+            data = 0
             for i in range(4):
-                data=data+resp[i+3]*pow(256,i)
+                data = data + resp[i + 3] * pow(256, i)
             return hex(data)
         if param == 0xE6:
-            data=0
+            data = 0
             for i in range(8):
-                data=data+resp[i+3]*pow(256,7-i)
+                data = data + resp[i + 3] * pow(256, 7 - i)
             return hex(data)
         l = len(resp)
-        data=""
-        for i in range(l-4):
-            data=data+chr(resp[i+3])
+        data = ""
+        for i in range(l - 4):
+            data = data + chr(resp[i + 3])
         return data
 
     def cmd_module_start(self):
         print("MODULE START")
-        self.serial_port.requestBroadcast(CMD_START,[])
+        self.serial_port.requestBroadcast(CMD_START, [])
 
     def cmd_module_stop(self):
         print("MODULE STOP")
-        self.serial_port.requestBroadcast(CMD_STOP,[])
+        self.serial_port.requestBroadcast(CMD_STOP, [])
 
     def cmd_reset(self):
         print("MODULE RESET")
-        resp = self.serial_port.request(self.module, 0, (SET+CMD_RESET),[], long_answer=0.3)
+        resp = self.serial_port.request(self.module, 0, (SET + CMD_RESET), [], long_answer=0.3)
 
     def cmd_store(self, sensor):
         print("MODULE STORE PARAM")
-        resp = self.serial_port.request(self.module, sensor, (SET+CMD_STORE),[], long_answer=0.1)
+        resp = self.serial_port.request(self.module, sensor, (SET + CMD_STORE), [], long_answer=0.1)
+
 
 if __name__ == "__main__":
-    
-    app = AppTest(0x05, 0x0)
-    #app.cmd_version()
-    #app.cmd_version()
-    # app.cmd_module_stop()
-    
-    if app.cmd_echo([97,98,99,100]) == 0:
+
+    nbus = NbusSystem(False)
+    slave1 = nbus.create_slave(5)
+    slave1.cmd_module_stop()
+    if slave1.cmd_echo([97, 98, 99, 100]) == 0:
         sys.exit()
 
-    #app.cmd_reset()
+    # app.cmd_reset()
     store_param = False
     if store_param:
-        app.cmd_set_param(1, PARAM_SAMPLERATE, 10)
-        app.cmd_set_param(1, PARAM_RANGE, 1)
-        app.cmd_set_param(1, PARAM_FILTER, 2)
+        slave1.cmd_set_param(1, PARAM_SAMPLERATE, 1)
+        slave1.cmd_set_param(1, PARAM_RANGE, 1)
+        slave1.cmd_set_param(1, PARAM_FILTER, 2)
+
+        slave1.cmd_store(1)
 
-        app.cmd_store(1)
-        
         sys.exit()
 
+    # !!!! slave1.cmd_set_param(1, PARAM_SAMPLERATE, 10)
+    # !!!! slave1.cmd_set_param(2, PARAM_SAMPLERATE, 10)
     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)
-    
+        sr = slave1.cmd_get_param(s + 1, PARAM_SAMPLERATE)
+        r = slave1.cmd_get_param(s + 1, PARAM_RANGE)
+        lpf = slave1.cmd_get_param(s + 1, PARAM_FILTER)
+        ee = slave1.cmd_get_param(s + 1, PARAM_GAIN)
+        print(sr, r, lpf, ee)
+
+    sr = slave1.cmd_get_param(1, PARAM_SAMPLERATE)
+
+    # time.sleep(0.5)
+    # app.cmd_module_stop()
+
+    slave1.cmd_module_start()
+    time.sleep(0.1)
+
+    # app.cmd_module_stop()
+    sys.exit()
+    pocet = slave1.cmd_sensor_cnt()
     # sys.exit()
-    #time.sleep(0.5)
-    #app.cmd_module_stop()
-    
-    app.cmd_module_start()
-    time.sleep(0.5)
-    
-    #app.cmd_module_stop()
-    #sys.exit()
-    pocet = app.cmd_sensor_cnt()
-    #sys.exit()
     print("pocet senzorov=", pocet)
-    #app.cmd_sensor_get_data_FSR(pocet)    
+    # app.cmd_sensor_get_data_FSR(pocet)
     for i in range(50):
-        acc = app.cmd_sensor_get_data_IMU(1)
-        gyr = app.cmd_sensor_get_data_IMU(2)
+        acc = slave1.cmd_sensor_get_data_IMU(1)
+        # gyr = slave1.cmd_sensor_get_data_IMU(2)
         print(acc)
-        print(gyr)
-    app.cmd_module_stop()
-    sys.exit()
-    
+        # print(gyr)
+    slave1.cmd_module_stop()
+    # sys.exit()
+
     for i in range(pocet):
-        d=app.cmd_sensor_get_data(i+1)
-        print("Data", i+1, ":\t", d)
-    
-
-    time.sleep(1)
-    app.cmd_module_stop()
-    
-    
-    
+        d = slave1.cmd_sensor_get_data(i + 1)
+        print("Data", i + 1, ":\t", d)
+
     print("Sensor type")
     for i in range(pocet):
-        print(app.cmd_sensor_type(i+1))
+        print(slave1.cmd_sensor_type(i + 1))
     print("Sensors type")
-    print(app.cmd_sensors_type(pocet))
-    par = app.cmd_sensor_get_params(1)
+    print(slave1.cmd_sensors_type(pocet))
+
+    par = slave1.cmd_get_params(1)
     print(par)
 
-    
-    r=app.cmd_module_info(0xE1)
-    print('NAME:' , r)    
-    r=app.cmd_module_info(0xE2)
+    r = slave1.cmd_module_info(0xE1)
+    print('NAME:', r)
+    r = slave1.cmd_module_info(0xE2)
     print('TYPE', r)
-        
-    r=app.cmd_module_info(0xE4)
+
+    r = slave1.cmd_module_info(0xE4)
     print('FW', r)
 
-    r=app.cmd_module_info(0xE5) 
+    r = slave1.cmd_module_info(0xE5)
     print('HW', r)
-    
-    r=app.cmd_module_info(0xE3)    
+
+    r = slave1.cmd_module_info(0xE3)
     print("UUID", r)
 
-    r=app.cmd_module_info(0xE6)    
+    r = slave1.cmd_module_info(0xE6)
     print("MEM ID", r)
-    app.finish()
-
+    sr = slave1.cmd_get_param(0, PARAM_SAMPLERATE)
+    nbus.finish()

test/comm.py

@@ -7,9 +7,6 @@ usleep = lambda x: time.sleep(x/1000000.0)
 SET = 0x20
 GET = 0x00
 
-MODULE_ADDR = 0x05
-
-
 
 CMD_ECHO = 0x01
 CMD_STOP = 0x02
@@ -93,7 +90,7 @@ def local_logger(*message):
 
 
 class SerialComm:
-    def __init__(self, port_name):
+    def __init__(self, port_name, enable_log = False):
         self.port = serial.Serial(timeout=0.05)
         if port_name != "":
             self.port.port = port_name
@@ -103,9 +100,14 @@ class SerialComm:
         self.port.parity = serial.PARITY_NONE
         self.port.baudrate = 921600
         self.comm_thread = None
-        self.callback = local_logger
+        self.callback_call = local_logger
         self.measure_active = False
         self.data_file = None
+        self.enable_logger = enable_log
+
+    def callback(self, *message):
+        if self.enable_logger:
+            self.callback_call(*message)
 
     def open(self):
         self.port.open()
@@ -166,8 +168,9 @@ class SerialComm:
     def requestBroadcast(self, command, data):
         m = self.create_packet([0, 0, 0, command], data)
         self.callback('d', 0, "\tRQ BC>", m)
-        self.port.write([m[0]]) # send legth of message
-        self.port.write(m[1:]) # send body of message
+        # self.port.write([m[0]]) # send legth of message
+        # self.port.write(m[1:]) # send body of message
+        self.port.write(m) # send body of message
 
     def request(self, module, sensor, command, data, skip_check=False, long_answer=0):
         m = self.create_packet([0, module, sensor, command], data)