Imu improvements

XRPLib/imu.py

@@ -7,6 +7,7 @@
 from machine import I2C, Pin, Timer, disable_irq, enable_irq
 import time, math
 
+LSM6DSO_WHO_AM_I = 0x0F
 LSM6DSO_CTRL1_XL = 0x10
 LSM6DSO_CTRL2_G = 0x11
 LSM6DSO_CTRL3_C = 0x12
@@ -26,6 +27,8 @@
 """
 LSM6DSO_SCALEA = ('2g', '16g', '4g', '8g')
 LSM6DSO_SCALEG = ('250', '125', '500', '', '1000', '', '2000')
+LSM6DSO_ODRA = ('0', '12.5', '26', '52', '104', '208', '416', '833', '1660', '3330', '6660')
+LSM6DSO_ODRG = ('0', '12.5', '26', '52', '104', '208', '416', '833', '1660', '3330', '6660')
 
 class IMU():
 
@@ -56,10 +59,24 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
         self._power = True
         self._power_a = 0x10
         self._power_g = 0x10
-        # ODR_XL=1 FS_XL=0
-        self._setreg(LSM6DSO_CTRL1_XL, 0x10)
-        # ODR_G=1 FS_125=1
-        self._setreg(LSM6DSO_CTRL2_G, 0x12)
+
+        # Check WHO_AM_I register to verify IMU is connected
+        foo = self._getreg(LSM6DSO_WHO_AM_I)
+        if foo != 0x6C:
+            # Getting here indicates sensor isn't connected
+            # TODO - do somehting intelligent here
+            pass
+
+        # Set SW_RESET and BOOT bits to completely reset the sensor
+        self._setreg(LSM6DSO_CTRL3_C, 0x81)
+        # Wait for register to return to default value, with timeout
+        t0 = time.ticks_ms()
+        timeout_ms = 100
+        while True:
+            foo = self._getreg(LSM6DSO_CTRL3_C)
+            if (foo == 0x04) or (time.ticks_ms() > (t0 + timeout_ms)):
+                break
+
         # BDU=1 IF_INC=1
         self._setreg(LSM6DSO_CTRL3_C, 0x44)
         self._setreg(LSM6DSO_CTRL8_XL, 0)
@@ -68,13 +85,15 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
         self._scale_g = 0
         self._scale_a_c = 1
         self._scale_g_c = 1
-        self.acc_scale('2g')
-        self.gyro_scale('500')
+        self.acc_scale('16g')
+        self.gyro_scale('2000')
+        self.acc_rate('208')
+        self.gyro_rate('208')
 
         self.gyro_offsets = [0,0,0]
         self.acc_offsets = [0,0,0]
 
-        self.update_time = 0.004
+        self.update_time = 0.005
         self.gyro_pitch_bias = 0
         self.adjusted_pitch = 0
 
@@ -321,6 +340,30 @@ def gyro_scale(self, dat=None):
             else: return
             self._r_w_reg(LSM6DSO_CTRL2_G, self._scale_g<<1, 0xF1)
 
+    def acc_rate(self, dat=None):
+        """
+        Set the accelerometer rate. The rate can be '0', '12.5', '26', '52', '104', '208', '416', '833', '1660', '3330', '6660'.
+        Pass in no parameters to retrieve the current value
+        """
+        if (dat is None) or (type(dat) is not str) or (dat not in LSM6DSO_ODRA):
+            reg_val = self._getreg(LSM6DSO_CTRL1_XL)
+            return (reg_val >> 4) & 0x04
+        else:
+            reg_val = LSM6DSO_ODRA.index(dat) << 4
+            return self._r_w_reg(LSM6DSO_CTRL1_XL, reg_val, 0x0F)
+
+    def gyro_rate(self, dat=None):
+        """
+        Set the gyroscope rate. The rate can be '0', '12.5', '26', '52', '104', '208', '416', '833', '1660', '3330', '6660'.
+        Pass in no parameters to retrieve the current value
+        """
+        if (dat is None) or (type(dat) is not str) or (dat not in LSM6DSO_ODRG):
+            reg_val = self._getreg(LSM6DSO_CTRL2_G)
+            return (reg_val >> 4) & 0x04
+        else:
+            reg_val = LSM6DSO_ODRG.index(dat) << 4
+            return self._r_w_reg(LSM6DSO_CTRL2_G, reg_val, 0x0F)
+
     def power(self, on:bool=None):
         """
         Turn the LSM6DSO on or off.
@@ -342,7 +385,7 @@ def power(self, on:bool=None):
                 self._r_w_reg(LSM6DSO_CTRL1_XL, 0, 0x0F)
                 self._r_w_reg(LSM6DSO_CTRL2_G, 0, 0x0F)
 
-    def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:int=4):
+    def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:int=5):
         """
         Collect readings for [calibration_time] seconds and calibrate the IMU based on those readings
         Do not move the robot during this time
@@ -356,22 +399,34 @@ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:
         :type update_time: int
         """
         self.update_timer.deinit()
-        start_time = time.ticks_ms()
         self.acc_offsets = [0,0,0]
         self.gyro_offsets = [0,0,0]
         avg_vals = [[0,0,0],[0,0,0]]
         num_vals = 0
+        # Wait a bit for sensor to start measuring (data registers may default to something nonsensical)
+        time.sleep(.1)
+        start_time = time.ticks_ms()
         while time.ticks_ms() < start_time + calibration_time*1000:
             cur_vals = self._get_acc_gyro_rates()
             # Accelerometer averages
-            avg_vals[0][0] = (avg_vals[0][0]*num_vals+cur_vals[0][0])/(num_vals+1)
-            avg_vals[0][1] = (avg_vals[0][1]*num_vals+cur_vals[0][1])/(num_vals+1)
-            avg_vals[0][2] = (avg_vals[0][2]*num_vals+cur_vals[0][2])/(num_vals+1)
+            avg_vals[0][0] += cur_vals[0][0]
+            avg_vals[0][1] += cur_vals[0][1]
+            avg_vals[0][2] += cur_vals[0][2]
             # Gyroscope averages
-            avg_vals[1][0] = (avg_vals[1][0]*num_vals+cur_vals[1][0])/(num_vals+1)
-            avg_vals[1][1] = (avg_vals[1][1]*num_vals+cur_vals[1][1])/(num_vals+1)
-            avg_vals[1][2] = (avg_vals[1][2]*num_vals+cur_vals[1][2])/(num_vals+1)
-            time.sleep(0.05)
+            avg_vals[1][0] += cur_vals[1][0]
+            avg_vals[1][1] += cur_vals[1][1]
+            avg_vals[1][2] += cur_vals[1][2]
+            # Increment counter and wait for next loop
+            num_vals += 1
+            time.sleep(update_time / 1000)
+
+        # Compute averages
+        avg_vals[0][0] /= num_vals
+        avg_vals[0][1] /= num_vals
+        avg_vals[0][2] /= num_vals
+        avg_vals[1][0] /= num_vals
+        avg_vals[1][1] /= num_vals
+        avg_vals[1][2] /= num_vals
 
         avg_vals[0][vertical_axis] -= 1000 #in mg