Retrieve stored calibration data (#17)

* Added new commands

* WIP

* wip

* - Moved examples to examples folder
- Tests added for getAllCalibration class
- Changes reformatted using PEP8

Author: MFernandezCarmona

examples/bt_api_example.py

@@ -0,0 +1,38 @@
+import time
+
+from serial import Serial
+
+from pyshimmer import ShimmerBluetooth, DEFAULT_BAUDRATE, DataPacket
+
+
+def stream_cb(pkt: DataPacket) -> None:   
+    print(f'Received new data packet: ') 
+    for chan in pkt.channels:
+        print(f'channel: ' + str(chan)) 
+        print(f'value: ' + str(pkt[chan]))     
+    print('') 
+
+def main(args=None):    
+    serial = Serial('/dev/rfcomm42', DEFAULT_BAUDRATE)
+    shim_dev = ShimmerBluetooth(serial)
+
+    shim_dev.initialize()
+
+    dev_name = shim_dev.get_device_name()
+    print(f'My name is: {dev_name}')
+
+    info = shim_dev.get_firmware_version() 
+    print("- firmware: [" + str(info[0]) + "]")
+    print("- version: [" + str(info[1].major) + "." + str(info[1].minor) + "." + str(info[1].rel) + "]")
+    
+    shim_dev.add_stream_callback(stream_cb)
+
+    shim_dev.start_streaming()
+    time.sleep(5.0)
+    shim_dev.stop_streaming()
+
+    shim_dev.shutdown()
+
+
+if __name__ == '__main__':
+    main()

examples/dock_example.py

@@ -0,0 +1,19 @@
+from serial import Serial
+
+from pyshimmer import ShimmerDock, DEFAULT_BAUDRATE, fmt_hex
+
+
+def main(args=None):    
+        serial = Serial('/dev/ttyECGdev', DEFAULT_BAUDRATE)
+
+        print(f'Connecting docker')
+        shim_dock = ShimmerDock(serial)
+
+        mac = shim_dock.get_mac_address()
+        print(f'Device MAC: {fmt_hex(mac)}')
+
+        shim_dock.close()
+
+
+if __name__ == '__main__':
+    main()        

pyshimmer/bluetooth/bt_api.py

@@ -24,7 +24,7 @@
     GetFirmwareVersionCommand, InquiryCommand, StartStreamingCommand, StopStreamingCommand, DataPacket, \
     GetEXGRegsCommand, SetEXGRegsCommand, StartLoggingCommand, StopLoggingCommand, GetExperimentIDCommand, \
     SetExperimentIDCommand, GetDeviceNameCommand, SetDeviceNameCommand, DummyCommand, GetBatteryCommand, \
-    SetSamplingRateCommand, SetSensorsCommand, SetStatusAckCommand
+    SetSamplingRateCommand, SetSensorsCommand, SetStatusAckCommand, AllCalibration, GetAllCalibrationCommand
 from pyshimmer.bluetooth.bt_const import ACK_COMMAND_PROCESSED, DATA_PACKET, FULL_STATUS_RESPONSE, INSTREAM_CMD_RESPONSE
 from pyshimmer.bluetooth.bt_serial import BluetoothSerial
 from pyshimmer.dev.channels import ChDataTypeAssignment, ChannelDataType, EChannelType, ESensorGroup
@@ -475,6 +475,12 @@ def get_exg_register(self, chip_id: int) -> ExGRegister:
         """
         return self._process_and_wait(GetEXGRegsCommand(chip_id))
 
+    def get_all_calibration(self) -> AllCalibration:
+        """Gets all calibration data from sensor
+        :return: An AllCalibration object that presents the calibration contents in an easily processable manner
+        """
+        return self._process_and_wait(GetAllCalibrationCommand())
+
     def set_exg_register(self, chip_id: int, offset: int, data: bytes) -> None:
         """Configure part of the memory of the ExG registers
 

pyshimmer/bluetooth/bt_commands.py

@@ -23,6 +23,7 @@
 from pyshimmer.dev.base import dr2sr, sr2dr, sec2ticks, ticks2sec
 from pyshimmer.dev.channels import ChannelDataType, EChannelType, ESensorGroup, serialize_sensorlist
 from pyshimmer.dev.exg import ExGRegister
+from pyshimmer.dev.calibration import AllCalibration
 from pyshimmer.dev.fw_version import get_firmware_type
 
 from pyshimmer.util import bit_is_set, resp_code_to_bytes, calibrate_u12_adc_value, battery_voltage_to_percent
@@ -221,7 +222,7 @@ def send(self, ser: BluetoothSerial) -> None:
 
     def receive(self, ser: BluetoothSerial) -> any:
         batt = ser.read_response(self.get_response_code(), arg_format='BBB')
-        # Calculation see: 
+        # Calculation see:
         # http://shimmersensing.com/wp-content/docs/support/documentation/LogAndStream_for_Shimmer3_Firmware_User_Manual_rev0.11a.pdf (Page 17)
         # https://shimmersensing.com/wp-content/docs/support/documentation/Shimmer_User_Manual_rev3p.pdf (Page 53)
         raw_values = batt[1] * 256 + batt[0]
@@ -337,11 +338,41 @@ def send(self, ser: BluetoothSerial) -> None:
         ser.write_command(GET_FW_VERSION_COMMAND)
 
     def receive(self, ser: BluetoothSerial) -> any:
-        fw_type_bin, major, minor, rel = ser.read_response(FW_VERSION_RESPONSE, arg_format='<HHBB')
+        fw_type_bin, major, minor, rel = ser.read_response(
+            FW_VERSION_RESPONSE, arg_format='<HHBB')
         fw_type = get_firmware_type(fw_type_bin)
         return fw_type, major, minor, rel
 
 
+class GetAllCalibrationCommand(ResponseCommand):
+    """ Returns all the stored calibration values (84 bytes) in the following order:
+
+            ESensorGroup.ACCEL_LN (21 bytes)
+            ESensorGroup.GYRO     (21 bytes)
+            ESensorGroup.MAG      (21 bytes)
+            ESensorGroup.ACCEL_WR (21 bytes)        
+
+        The breakdown of the kinematic (accel x 2, gyro and mag) calibration values is as follows:
+            [bytes  0- 5] offset bias values: 3 (x,y,z) 16-bit signed integers (big endian). 
+            [bytes  6-11] sensitivity values: 3 (x,y,z) 16-bit signed integers (big endian). 
+            [bytes 12-20] alignment matrix:  9 values    8-bit signed integers.
+    """
+
+    def __init__(self):
+        super().__init__(ALL_CALIBRATION_RESPONSE)
+
+        self._offset = 0x0
+        self._rlen = 0x54  # 84 bytes
+
+    def send(self, ser: BluetoothSerial) -> None:
+        ser.write_command(GET_ALL_CALIBRATION_COMMAND)
+
+    def receive(self, ser: BluetoothSerial) -> any:
+        ser.read_response(ALL_CALIBRATION_RESPONSE)
+        reg_data = ser.read(self._rlen)
+        return AllCalibration(reg_data)
+
+
 class InquiryCommand(ResponseCommand):
     """Perform an inquiry to determine the sample rate, buffer size, and active data channels
 
@@ -360,7 +391,8 @@ def send(self, ser: BluetoothSerial) -> None:
         ser.write_command(INQUIRY_COMMAND)
 
     def receive(self, ser: BluetoothSerial) -> any:
-        sr_val, _, n_ch, buf_size = ser.read_response(INQUIRY_RESPONSE, arg_format='<HIBB')
+        sr_val, _, n_ch, buf_size = ser.read_response(
+            INQUIRY_RESPONSE, arg_format='<HIBB')
         channel_conf = ser.read(n_ch)
 
         sr = dr2sr(sr_val)
@@ -403,12 +435,14 @@ def __init__(self, chip_id: int):
         self._rlen = 0xA
 
     def send(self, ser: BluetoothSerial) -> None:
-        ser.write_command(GET_EXG_REGS_COMMAND, 'BBB', self._chip, self._offset, self._rlen)
+        ser.write_command(GET_EXG_REGS_COMMAND, 'BBB',
+                          self._chip, self._offset, self._rlen)
 
     def receive(self, ser: BluetoothSerial) -> any:
         rlen = ser.read_response(EXG_REGS_RESPONSE, arg_format='B')
         if not rlen == self._rlen:
-            raise ValueError('Response does not contain required amount of bytes')
+            raise ValueError(
+                'Response does not contain required amount of bytes')
 
         reg_data = ser.read(rlen)
         return ExGRegister(reg_data)
@@ -429,7 +463,8 @@ def __init__(self, chip_id: int, offset: int, data: bytes):
 
     def send(self, ser: BluetoothSerial) -> None:
         dlen = len(self._data)
-        ser.write_command(SET_EXG_REGS_COMMAND, 'BBB', self._chip, self._offset, dlen)
+        ser.write_command(SET_EXG_REGS_COMMAND, 'BBB',
+                          self._chip, self._offset, dlen)
         ser.write(self._data)
 
 

pyshimmer/bluetooth/bt_const.py

@@ -84,6 +84,9 @@
 
 ENABLE_STATUS_ACK_COMMAND = 0xA3
 
+GET_ALL_CALIBRATION_COMMAND = 0x2C
+ALL_CALIBRATION_RESPONSE = 0x2D
+
 """
 The Bluetooth LogAndStream API assigns a numerical index to each channel type. This dictionary maps each index to the
 corresponding channel type.

pyshimmer/dev/calibration.py

@@ -0,0 +1,85 @@
+# pyshimmer - API for Shimmer sensor devices
+# Copyright (C) 2023  Lukas Magel, Manuel Fernandez-Carmona
+
+# This program is free software: you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+
+# You should have received a copy of the GNU General Public License
+# along with this program.  If not, see <https://www.gnu.org/licenses/>.
+
+import struct
+from typing import List
+
+from pyshimmer.util import fmt_hex
+
+
+class AllCalibration:
+
+    def __init__(self, reg_bin: bytes):
+        self._num_bytes = 84
+        self._sensor_bytes = 21
+        self._num_sensors = 4
+
+        if len(reg_bin) < self._num_bytes:
+            raise ValueError(
+                f'All calibration data must have length {self._num_bytes}')
+
+        self._reg_bin = reg_bin
+
+    def __str__(self) -> str:
+        def print_sensor(sens_num: int) -> str:
+            return f'Sensor {sens_num + 1:2d}\n' + \
+                   f'\tOffset bias: {self.get_offset_bias(sens_num)}\n' + \
+                   f'\tSensitivity: {self.get_sensitivity(sens_num)}\n' + \
+                   f'\tAlignment Matrix: {self.get_ali_mat(sens_num)}\n'
+
+        obj_str = f''
+        for i in range(0, self._num_sensors):
+            obj_str += print_sensor(i)
+
+        reg_bin_str = fmt_hex(self._reg_bin)
+        obj_str += f'Binary: {reg_bin_str}\n'
+
+        return obj_str
+
+    @property
+    def binary(self):
+        return self._reg_bin
+    
+    def __eq__(self, other: "AllCalibration") -> bool:
+        return self._reg_bin == other._reg_bin
+
+    def _check_sens_num(self, sens_num: int) -> None:
+        if not 0 <= sens_num < (self._num_sensors):
+            raise ValueError(f'Sensor num must be 0 to {self._num_sensors-1}')
+
+    def get_offset_bias(self, sens_num: int) -> List[int]:
+        self._check_sens_num(sens_num)
+        start_offset = sens_num * self._sensor_bytes
+        end_offset = start_offset + 6
+        ans = list(struct.unpack(
+            '>hhh', self._reg_bin[start_offset:end_offset]))
+        return ans
+
+    def get_sensitivity(self, sens_num: int) -> List[int]:
+        self._check_sens_num(sens_num)
+        start_offset = sens_num * self._sensor_bytes + 6
+        end_offset = start_offset + 6
+        ans = list(struct.unpack(
+            '>hhh', self._reg_bin[start_offset:end_offset]))
+        return ans
+
+    def get_ali_mat(self, sens_num: int) -> List[int]:
+        self._check_sens_num(sens_num)
+        start_offset = sens_num * self._sensor_bytes + 12
+        end_offset = start_offset + 9
+        ans = list(struct.unpack(
+            '>bbbbbbbbb', self._reg_bin[start_offset:end_offset]))
+        return ans

test/bluetooth/test_bt_commands.py

@@ -21,7 +21,7 @@
     GetFirmwareVersionCommand, InquiryCommand, StartStreamingCommand, StopStreamingCommand, StartLoggingCommand, \
     StopLoggingCommand, GetEXGRegsCommand, SetEXGRegsCommand, GetExperimentIDCommand, SetExperimentIDCommand, \
     GetDeviceNameCommand, SetDeviceNameCommand, DummyCommand, DataPacket, ResponseCommand, SetStatusAckCommand, \
-    SetSensorsCommand, SetSamplingRateCommand
+    SetSensorsCommand, SetSamplingRateCommand, GetAllCalibrationCommand
 from pyshimmer.bluetooth.bt_serial import BluetoothSerial
 from pyshimmer.dev.channels import ChDataTypeAssignment, EChannelType, ESensorGroup
 from pyshimmer.dev.fw_version import EFirmwareType
@@ -95,10 +95,12 @@ def test_set_sampling_rate_command(self):
 
     def test_get_battery_state_command(self):
         cmd = GetBatteryCommand(in_percent=True)
-        self.assert_cmd(cmd, b'\x95', b'\x8a\x94', b'\x8a\x94\x30\x0b\x80', 100)
+        self.assert_cmd(cmd, b'\x95', b'\x8a\x94',
+                        b'\x8a\x94\x30\x0b\x80', 100)
 
         cmd = GetBatteryCommand(in_percent=False)
-        self.assert_cmd(cmd, b'\x95', b'\x8a\x94', b'\x8a\x94\x2e\x0b\x80', 4.168246153846154)
+        self.assert_cmd(cmd, b'\x95', b'\x8a\x94',
+                        b'\x8a\x94\x2e\x0b\x80', 4.168246153846154)
 
     def test_set_sensors_command(self):
         sensors = [
@@ -119,7 +121,8 @@ def test_set_config_time_command(self):
 
     def test_get_rtc(self):
         cmd = GetRealTimeClockCommand()
-        r = self.assert_cmd(cmd, b'\x91', b'\x90', b'\x90\x1f\xb1\x93\x09\x00\x00\x00\x00')
+        r = self.assert_cmd(cmd, b'\x91', b'\x90',
+                            b'\x90\x1f\xb1\x93\x09\x00\x00\x00\x00')
         self.assertAlmostEqual(r, 4903.3837585)
 
     def test_set_rtc(self):
@@ -129,19 +132,22 @@ def test_set_rtc(self):
     def test_get_status_command(self):
         cmd = GetStatusCommand()
         expected_result = [True, False, True, False, False, True, False, False]
-        self.assert_cmd(cmd, b'\x72', b'\x8a\x71', b'\x8a\x71\x25', expected_result)
+        self.assert_cmd(cmd, b'\x72', b'\x8a\x71',
+                        b'\x8a\x71\x25', expected_result)
 
     def test_get_firmware_version_command(self):
         cmd = GetFirmwareVersionCommand()
-        fw_type, major, minor, patch = self.assert_cmd(cmd, b'\x2e', b'\x2f', b'\x2f\x03\x00\x00\x00\x0b\x00')
+        fw_type, major, minor, patch = self.assert_cmd(
+            cmd, b'\x2e', b'\x2f', b'\x2f\x03\x00\x00\x00\x0b\x00')
         self.assertEqual(fw_type, EFirmwareType.LogAndStream)
         self.assertEqual(major, 0)
         self.assertEqual(minor, 11)
         self.assertEqual(patch, 0)
 
     def test_inquiry_command(self):
         cmd = InquiryCommand()
-        sr, buf_size, ctypes = self.assert_cmd(cmd, b'\x01', b'\x02', b'\x02\x40\x00\x01\xff\x01\x09\x01\x01\x12')
+        sr, buf_size, ctypes = self.assert_cmd(
+            cmd, b'\x01', b'\x02', b'\x02\x40\x00\x01\xff\x01\x09\x01\x01\x12')
 
         self.assertEqual(sr, 512.0)
         self.assertEqual(buf_size, 1)
@@ -165,7 +171,8 @@ def test_stop_logging_command(self):
 
     def test_get_exg_register_command(self):
         cmd = GetEXGRegsCommand(1)
-        r = self.assert_cmd(cmd, b'\x63\x01\x00\x0a', b'\x62', b'\x62\x0a\x00\x80\x10\x00\x00\x00\x00\x00\x02\x01')
+        r = self.assert_cmd(cmd, b'\x63\x01\x00\x0a', b'\x62',
+                            b'\x62\x0a\x00\x80\x10\x00\x00\x00\x00\x00\x02\x01')
         self.assertEqual(r.binary, b'\x00\x80\x10\x00\x00\x00\x00\x00\x02\x01')
 
     def test_get_exg_reg_fail(self):
@@ -175,6 +182,11 @@ def test_get_exg_reg_fail(self):
         mock.test_put_read_data(b'\x62\x04\x01\x02\x03\x04')
         self.assertRaises(ValueError, cmd.receive, serial)
 
+    def test_get_allcalibration_command(self):
+        cmd = GetAllCalibrationCommand()
+        r = self.assert_cmd(cmd, b'\x2c', b'\x2d', b'\x2d\x08\xcd\x08\xcd\x08\xcd\x00\x5c\x00\x5c\x00\x5c\x00\x9c\x00\x9c\x00\x00\x00\x00\x9c\x00\x00\x00\x00\x00\x00\x19\x96\x19\x96\x19\x96\x00\x9c\x00\x9c\x00\x00\x00\x00\x9c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x06\x87\x06\x87\x06\x87\x00\x9c\x00\x64\x00\x00\x00\x00\x9c')
+        self.assertEqual(r.binary, b'\x08\xcd\x08\xcd\x08\xcd\x00\x5c\x00\x5c\x00\x5c\x00\x9c\x00\x9c\x00\x00\x00\x00\x9c\x00\x00\x00\x00\x00\x00\x19\x96\x19\x96\x19\x96\x00\x9c\x00\x9c\x00\x00\x00\x00\x9c\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x06\x87\x06\x87\x06\x87\x00\x9c\x00\x64\x00\x00\x00\x00\x9c')
+
     def test_set_exg_register_command(self):
         cmd = SetEXGRegsCommand(1, 0x02, b'\x10\x00')
         self.assert_cmd(cmd, b'\x61\x01\x02\x02\x10\x00')