Uart

adafruit_bno080/__init__.py

@@ -31,14 +31,13 @@
 from collections import namedtuple
 import time
 from micropython import const
-import digitalio
 
 # TODO: Remove on release
 from .debug import channels, reports
 
 # TODO: shorten names
 # Channel 0: the SHTP command channel
-_BNO_CHANNEL_SHTP_COMMAND = const(0)
+BNO_CHANNEL_SHTP_COMMAND = const(0)
 BNO_CHANNEL_EXE = const(1)
 _BNO_CHANNEL_CONTROL = const(2)
 _BNO_CHANNEL_INPUT_SENSOR_REPORTS = const(3)
@@ -450,7 +449,7 @@ def shake(self):
     def _process_available_packets(self):
         processed_count = 0
         while self._data_ready:
-            print("reading a packet")
+            #print("reading a packet")
             try:
                 new_packet = self._read_packet()
             except PacketError:
@@ -480,8 +479,9 @@ def _wait_for_packet_type(self, channel_number, report_id=None, timeout=5.0):
                         return new_packet
                 else:
                     return new_packet
-            self._dbg("passing packet to handler for de-slicing")
-            self._handle_packet(new_packet)
+            if new_packet.channel_number not in (BNO_CHANNEL_EXE, BNO_CHANNEL_SHTP_COMMAND):
+                self._dbg("passing packet to handler for de-slicing")
+                self._handle_packet(new_packet)
 
         raise RuntimeError("Timed out waiting for a packet on channel", channel_number)
 
@@ -503,11 +503,14 @@ def _update_sequence_number(self, new_packet):
         self._sequence_number[channel] = seq
 
     def _handle_packet(self, packet):
-
         # split out reports first
-        _separate_batch(packet, self._packet_slices)
-        while len(self._packet_slices) > 0:
-            self._process_report(*self._packet_slices.pop())
+        try:
+            _separate_batch(packet, self._packet_slices)
+            while len(self._packet_slices) > 0:
+                self._process_report(*self._packet_slices.pop())
+        except Exception as e:
+            print(packet)
+            raise e
 
     def _handle_control_report(self, report_id, report_bytes):
         if report_id == _SHTP_REPORT_PRODUCT_ID_RESPONSE:
@@ -543,8 +546,11 @@ def _process_report(self, report_id, report_bytes):
             if report_id == BNO_REPORT_SHAKE_DETECTOR:
                 shake_detected = _parse_shake_report(report_bytes)
                 # shake not previously detected - auto cleared by 'shake' property
-                if not self._readings[BNO_REPORT_SHAKE_DETECTOR]:
-                    self._readings[BNO_REPORT_SHAKE_DETECTOR] = shake_detected
+                try:
+                    if not self._readings[BNO_REPORT_SHAKE_DETECTOR]:
+                        self._readings[BNO_REPORT_SHAKE_DETECTOR] = shake_detected
+                except KeyError:
+                    pass
                 return
 
             sensor_data = _parse_sensor_report_data(report_bytes)
@@ -571,12 +577,12 @@ def enable_feature(self, feature_id):
         self._dbg("\n********** Enabling feature id:", feature_id, "**********")
 
         set_feature_report = self._get_feature_enable_report(feature_id)
-        print("Enabling", feature_id)
+        #print("Enabling", feature_id)
         self._send_packet(_BNO_CHANNEL_CONTROL, set_feature_report)
         while True:
             packet = self._wait_for_packet_type(
                 _BNO_CHANNEL_CONTROL, _BNO_CMD_GET_FEATURE_RESPONSE
-            )
+                )
 
             if packet.data[1] == feature_id:
                 if (
@@ -585,7 +591,7 @@ def enable_feature(self, feature_id):
                     self._readings[feature_id] = (0.0, 0.0, 0.0, 0.0)
                 else:
                     self._readings[feature_id] = (0.0, 0.0, 0.0)
-                print("Enabled", feature_id)
+                #print("Enabled", feature_id)
                 break
             else:
                 raise RuntimeError("Was not able to enable feature", feature_id)
@@ -652,6 +658,7 @@ def hard_reset(self):
         if not self._reset:
             return
         #print("Hard resetting...")
+        import digitalio
         self._reset.direction = digitalio.Direction.OUTPUT
         self._reset.value = True
         time.sleep(0.01)
@@ -667,22 +674,16 @@ def soft_reset(self):
         data[0] = 1
         seq = self._send_packet(BNO_CHANNEL_EXE, data)
         time.sleep(0.5)
-
+        seq = self._send_packet(BNO_CHANNEL_EXE, data)
+        time.sleep(0.5)
+
         for i in range(3):
-            while True:  # retry reading packets until ready!
-                try:
-                    packet = self._read_packet()
-                    break
-                except PacketError:
-                    time.sleep(0.1)
-
-            #print(packet)
-            if i == 0 and packet.channel_number != _BNO_CHANNEL_SHTP_COMMAND:
-                raise RuntimeError("Expected an SHTP announcement")
-            if i == 1 and packet.channel_number != BNO_CHANNEL_EXE:
-                raise RuntimeError("Expected a reset reply")
-            if i == 2 and packet.channel_number != _BNO_CHANNEL_CONTROL:
-                raise RuntimeError("Expected a control announcement")
+            try:
+                packet = self._read_packet()
+            except PacketError:
+                time.sleep(0.5)
+
+
         print("OK!");
         # all is good!
 

adafruit_bno080/i2c.py

@@ -11,11 +11,8 @@
 import adafruit_bus_device.i2c_device as i2c_device
 from . import BNO080, BNO_CHANNEL_EXE, DATA_BUFFER_SIZE, const, Packet, PacketError
 
-# should be removeable; I _think_ something else should be able to prep the buffers?
-
 _BNO080_DEFAULT_ADDRESS = const(0x4A)
 
-
 class BNO080_I2C(BNO080):
     """Library for the BNO080 IMU from Hillcrest Laboratories
 
@@ -33,10 +30,7 @@ def _send_packet(self, channel, data):
 
         pack_into("<H", self._data_buffer, 0, write_length)
         self._data_buffer[2] = channel
-
         self._data_buffer[3] = self._sequence_number[channel]
-
-        # this is dumb but it's what we have for now
         for idx, send_byte in enumerate(data):
             self._data_buffer[4 + idx] = send_byte
 
@@ -58,8 +52,6 @@ def _read_header(self):
         return packet_header
 
     def _read_packet(self):
-        # TODO: MAGIC NUMBER?
-        # TODO: this can be `_read_header` or know it's done by `_data_ready`
         with self.bus_device_obj as i2c:
             i2c.readinto(self._data_buffer, end=4)  # this is expecting a header?
         self._dbg("")
@@ -85,7 +77,6 @@ def _read_packet(self):
 
         self._read(packet_byte_count)
 
-        # TODO: Allocation
         new_packet = Packet(self._data_buffer)
         if self._debug:
             print(new_packet)
@@ -114,7 +105,6 @@ def _data_ready(self):
 
         if header.channel_number > 5:
             self._dbg("channel number out of range:", header.channel_number)
-        #  data_length, packet_byte_count)
         if header.packet_byte_count == 0x7FFF:
             print("Byte count is 0x7FFF/0xFFFF; Error?")
             if header.sequence_number == 0xFF: