ban-module/test/app.py

import sys
from nbus_slave import *
from nbus_system import *
import matplotlib.pyplot as plt
from imu_slave import *
import numpy as np

def show_data(data):
    fig, ax = plt.subplots()
    for d in data:
        ax.plot(data[d])
    ax.grid()
    plt.show()


if __name__ == "__main__":

    nbus = NbusSystem(True)
    s1_adr = 6
    s2_adr = 9
    adrs = [s1_adr, s2_adr]

    foundSlaves = {}
    for adr in adrs:
        slave1 = nbus.create_slave(ImuSlave, adr)
        foundSlaves[adr] = True

    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()
    store_param = False
    if store_param:
        nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_SAMPLERATE, 1)
        nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_RANGE, 0)
        nbus.get_slave(s1_adr).cmd_set_param(1, PARAM_FILTER, 2)

        nbus.get_slave(s1_adr).cmd_store(1)

        sys.exit()

    # !!!! slave1.cmd_set_param(1, PARAM_SAMPLERATE, 10)
    # !!!! slave1.cmd_set_param(2, PARAM_SAMPLERATE, 10)
    # for adr in adrs:
    #     for s in range(1):
    #         sr = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_SAMPLERATE)
    #         r = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_RANGE)
    #         lpf = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_FILTER)
    #         ee = nbus.get_slave(adr).cmd_get_param(s + 1, PARAM_GAIN)
    #         print(sr, r, lpf, ee)

    pocet = {}
    for adr in adrs:
        pocet[adr] = nbus.get_slave(adr).cmd_sensor_cnt()
        # sys.exit()
    print("pocet senzorov=", pocet)

    types = {}
    for adr in adrs:
        types[adr] = nbus.get_slave(adr).cmd_sensors_type()
    print("TYPES=", types)

    info = {}
    for adr in adrs:
        info[adr] = nbus.get_slave(adr).cmd_sensor_info()
    print("INFO=", info)

    info = {}
    for adr in adrs:
        info[adr] = nbus.get_slave(adr).cmd_sensors_format()
    print("FORMAT=", info)

    
    nbus.cmd_start()
    time.sleep(1)
    nbus.cmd_stop()
    
    sys.exit()
    data_acc = {}
    for adr in adrs:
        data_acc[adr] = []
    print(data_acc)
    start = time.time()
    num = 56
    for i in range(num):
        for adr in adrs:
            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=" ")
    stop = time.time() - start
    print(f"t={stop}")
    print(f"datarate={num/stop} mer/s")
    nbus.cmd_stop()
    nbus.cmd_stop()
    show_data(data_acc)
    nbus.cmd_stop()
    nbus.finish()
    sys.exit()

    for adr in adrs:
        for i in range(pocet[adr]):
            d = nbus.get_slave(adr).cmd_sensor_get_data(i + 1)
            print("Data", i + 1, ":\t", d)

        print("Sensor type")
        for i in range(pocet[adr]):
            print(nbus.get_slave(adr).cmd_sensor_type(i + 1))
        print("Sensors type")
        print(nbus.get_slave(adr).cmd_sensors_type(pocet[adr]))

        par = nbus.get_slave(adr).cmd_get_params(1)
        print(par)

        r = nbus.get_slave(adr).cmd_module_info(0xE1)
        print('NAME:', r)
        r = nbus.get_slave(adr).cmd_module_info(0xE2)
        print('TYPE', r)

        r = nbus.get_slave(adr).cmd_module_info(0xE4)
        print('FW', r)

        r = nbus.get_slave(adr).cmd_module_info(0xE5)
        print('HW', r)

        r = nbus.get_slave(adr).cmd_module_info(0xE3)
        print("UUID", r)

        r = nbus.get_slave(adr).cmd_module_info(0xE6)
        print("MEM ID", r)

    nbus.finish()