ban-module/test/app.py

from comm import *
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

    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)        
        echo = ""
        if len(resp) == 0:
            print("No ECHO (0-size resp)")
            return 0
        print(resp)
        for r in range(len(msg)):
            echo = echo + chr(resp[3+r])
        print("Echo:"+echo)
        return len(resp)

    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_params(self, sensor):
        print("GET params:")
        resp = self.serial_port.request(self.module, sensor, (CMD_PARAM),[])    

    def cmd_sensor_cnt(self):
        print("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,[])
        return resp[3]

    def cmd_sensors_type(self, pocet):
        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]])
        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:
            h = resp[5] * 256 + resp[4]
        # print(hex(h))        
            return h/4096*3.3
        return 0

    def cmd_sensor_get_data_IMU(self, sensor_index):
        '''
        sensor_index:
        1 - ACC
        2 - GYRO
        '''
        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 x > 32768:
                x = x - 65535
            if y > 32768:
                y = y - 65535                
            if z > 32768:
                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,[])
        print(resp)
        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)
        
        

    def cmd_module_info(self, param):
        resp = self.serial_port.request(self.module, 0, CMD_INFO,[param])
        if param == 0xE3:
            data=0
            for i in range(4):
                data=data+resp[i+3]*pow(256,i)
            return hex(data)
        if param == 0xE6:
            data=0
            for i in range(8):
                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])
        return data

    def cmd_module_start(self):
        print("MODULE START")
        self.serial_port.requestBroadcast(CMD_START,[])

    def cmd_module_stop(self):
        print("MODULE 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)

    def cmd_store(self, sensor):
        print("MODULE STORE PARAM")
        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:
        sys.exit()

    #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)

        app.cmd_store(1)
        
        sys.exit()

    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()
    
    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)    
    for i in range(50):
        acc = app.cmd_sensor_get_data_IMU(1)
        gyr = app.cmd_sensor_get_data_IMU(2)
        print(acc)
        print(gyr)
    app.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()
    
    
    
    print("Sensor type")
    for i in range(pocet):
        print(app.cmd_sensor_type(i+1))
    print("Sensors type")
    print(app.cmd_sensors_type(pocet))
    par = app.cmd_sensor_get_params(1)
    print(par)

    
    r=app.cmd_module_info(0xE1)
    print('NAME:' , r)    
    r=app.cmd_module_info(0xE2)
    print('TYPE', r)
        
    r=app.cmd_module_info(0xE4)
    print('FW', r)

    r=app.cmd_module_info(0xE5) 
    print('HW', r)
    
    r=app.cmd_module_info(0xE3)    
    print("UUID", r)

    r=app.cmd_module_info(0xE6)    
    print("MEM ID", r)
    app.finish()