From 406af89e92566a01d40238abf3038cb147f47917 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 11:44:07 -0600
Subject: [PATCH 01/22] Improve IMU calibration
Actually increment num_vals so we're using more than just the last measurement
Add short delay before calibration loop to ensure IMU has started measurements
Delay measurements by update_time rather than a magic number
Optimize calibration loop by doing division after the loop finishes
---
XRPLib/imu.py | 26 +++++++++++++++++++-------
1 file changed, 19 insertions(+), 7 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 8353ff3..9cd589e 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -361,17 +361,29 @@ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:
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)
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
From 4970b76d7ebe869f5bc6fbc6b8bddb19bb8d1993 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 11:49:00 -0600
Subject: [PATCH 02/22] Change default ODR and FS
Change ODR to 208Hz for both accelerometer and gyroscope
Change default update_time to 5ms (close as possible to 208Hz)
Change FS range of both sensors to maximum (not clipping is more important than fine resolution here)
---
XRPLib/imu.py | 12 ++++++------
1 file changed, 6 insertions(+), 6 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 9cd589e..02543c0 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -56,10 +56,10 @@ 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)
+ # Set accelerometer ODR=208Hz and FS=16g
+ self._setreg(LSM6DSO_CTRL1_XL, 0x54)
+ # Set gyroscope ODR=208Hz and FS=2000dps
+ self._setreg(LSM6DSO_CTRL2_G, 0x5C)
# BDU=1 IF_INC=1
self._setreg(LSM6DSO_CTRL3_C, 0x44)
self._setreg(LSM6DSO_CTRL8_XL, 0)
@@ -74,7 +74,7 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
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
@@ -342,7 +342,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
From 767dd78d00afc142cb83460ed43adabb0395a736 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 11:50:52 -0600
Subject: [PATCH 03/22] Improve IMU initialization
Check WHO_AM_I register (should do something intelligent if it fails)
Set RESET and BOOT bits to reset sensor during initialization
---
XRPLib/imu.py | 19 +++++++++++++++++++
1 file changed, 19 insertions(+)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 02543c0..f13ff7e 100644
--- a/XRPLib/imu.py
+++ b/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
@@ -56,6 +57,24 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self._power = True
self._power_a = 0x10
self._power_g = 0x10
+
+ # 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
+
# Set accelerometer ODR=208Hz and FS=16g
self._setreg(LSM6DSO_CTRL1_XL, 0x54)
# Set gyroscope ODR=208Hz and FS=2000dps
From ae61c14499e3fcfab5ecf7af369ce495c6a67dae Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 15:11:32 -0600
Subject: [PATCH 04/22] Fix IMU scales
Function calls were overriding previously set values
---
XRPLib/imu.py | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index f13ff7e..72d55bf 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -87,8 +87,8 @@ 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.gyro_offsets = [0,0,0]
self.acc_offsets = [0,0,0]
From e22e1da36db756414a6958ef90f49e2db60a7d15 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 15:13:12 -0600
Subject: [PATCH 05/22] Add IMU rate functions
---
XRPLib/imu.py | 32 ++++++++++++++++++++++++++++----
1 file changed, 28 insertions(+), 4 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 72d55bf..aa6f482 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -27,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():
@@ -75,10 +77,6 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
if (foo == 0x04) or (time.ticks_ms() > (t0 + timeout_ms)):
break
- # Set accelerometer ODR=208Hz and FS=16g
- self._setreg(LSM6DSO_CTRL1_XL, 0x54)
- # Set gyroscope ODR=208Hz and FS=2000dps
- self._setreg(LSM6DSO_CTRL2_G, 0x5C)
# BDU=1 IF_INC=1
self._setreg(LSM6DSO_CTRL3_C, 0x44)
self._setreg(LSM6DSO_CTRL8_XL, 0)
@@ -89,6 +87,8 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self._scale_g_c = 1
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]
@@ -340,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, 0xF0)
+
+ 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, 0xF0)
+
def power(self, on:bool=None):
"""
Turn the LSM6DSO on or off.
From 331481e2047c3c74e1286cb34c2e9ce5fe185200 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 15:26:49 -0600
Subject: [PATCH 06/22] Fix start_time in IMU calibration
Should really set the start time *after* waiting for the sensor to get going
---
XRPLib/imu.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index aa6f482..f61b7b4 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -399,13 +399,13 @@ 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
From 8d9fc59d18c94c08525a44360b819ddbe4628d85 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Thu, 13 Jul 2023 16:28:11 -0600
Subject: [PATCH 07/22] Fix bit masks in IMU rate functions
---
XRPLib/imu.py | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index f61b7b4..2c07b4f 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -350,7 +350,7 @@ def acc_rate(self, dat=None):
return (reg_val >> 4) & 0x04
else:
reg_val = LSM6DSO_ODRA.index(dat) << 4
- return self._r_w_reg(LSM6DSO_CTRL1_XL, reg_val, 0xF0)
+ return self._r_w_reg(LSM6DSO_CTRL1_XL, reg_val, 0x0F)
def gyro_rate(self, dat=None):
"""
@@ -362,7 +362,7 @@ def gyro_rate(self, dat=None):
return (reg_val >> 4) & 0x04
else:
reg_val = LSM6DSO_ODRG.index(dat) << 4
- return self._r_w_reg(LSM6DSO_CTRL2_G, reg_val, 0xF0)
+ return self._r_w_reg(LSM6DSO_CTRL2_G, reg_val, 0x0F)
def power(self, on:bool=None):
"""
From c16cadb5a35850975fd1701805905424464669b2 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 13:38:04 -0600
Subject: [PATCH 08/22] (UNTESTED) Refactor IMU definitions
Add new imu_defs.py with the following:
Both possible I2C addresses
Register address definitions
Struct layout definitions with bit fields to match registers
Dictionaries with possible ODR and FS values
Note - to reduce file size, only registers that are used have been included. It should be easy to expand these as needed
Add the following member functions:
is_connected()
reset()
_set_bdu()
_set_if_inc()
Refactor imu.py to use these new features
---
XRPLib/imu.py | 232 ++++++++++++++++++++++++++++-----------------
XRPLib/imu_defs.py | 76 +++++++++++++++
2 files changed, 219 insertions(+), 89 deletions(-)
create mode 100644 XRPLib/imu_defs.py
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 2c07b4f..7151eb5 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -4,32 +4,11 @@
# Author: shaoziyang (shaoziyang@micropython.org.cn)
# v1.0 2019.7
+from .imu_defs import *
+from uctypes import struct, addressof
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
-LSM6DSO_CTRL6_C = 0x15
-LSM6DSO_CTRL8_XL = 0x17
-LSM6DSO_STATUS = 0x1E
-LSM6DSO_OUT_TEMP_L = 0x20
-LSM6DSO_OUTX_L_G = 0x22
-LSM6DSO_OUTY_L_G = 0x24
-LSM6DSO_OUTZ_L_G = 0x26
-LSM6DSO_OUTX_L_A = 0x28
-LSM6DSO_OUTY_L_A = 0x2A
-LSM6DSO_OUTZ_L_A = 0x2C
-
-"""
- Options for accelerometer and gyroscope scale factors
-"""
-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():
_DEFAULT_IMU_INSTANCE = None
@@ -44,7 +23,7 @@ def get_default_imu(cls):
cls._DEFAULT_IMU_INSTANCE = cls(
scl_pin=19,
sda_pin=18,
- addr=0x6B
+ addr=LSM_ADDR_PRIMARY
)
cls._DEFAULT_IMU_INSTANCE.calibrate()
return cls._DEFAULT_IMU_INSTANCE
@@ -59,27 +38,27 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self._power = True
self._power_a = 0x10
self._power_g = 0x10
-
- # 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
+
+ # Copies of registers. Bytes and structs share the same memory
+ # addresses, so changing one changes the other
+ self.reg_ctrl1_xl_byte = bytearray(1)
+ self.reg_ctrl2_g_byte = bytearray(1)
+ self.reg_ctrl3_c_byte = bytearray(1)
+ self.reg_ctrl1_xl_struct = struct(addressof(self.reg_ctrl1_xl_byte), LSM_REG_LAYOUT_CTRL1_XL)
+ self.reg_ctrl2_g_struct = struct(addressof(self.reg_ctrl2_g_byte), LSM_REG_LAYOUT_CTRL2_G)
+ self.reg_ctrl3_c_struct = struct(addressof(self.reg_ctrl3_c_byte), LSM_REG_LAYOUT_CTRL3_C)
+
+ # Check if the IMU is connected
+ if not self.is_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
+ # Reset sensor to clear any previous configuration
+ self.reset()
- # BDU=1 IF_INC=1
- self._setreg(LSM6DSO_CTRL3_C, 0x44)
- self._setreg(LSM6DSO_CTRL8_XL, 0)
+ # Enable block data update
+ self._set_bdu()
+
# scale=2G
self._scale_a = 0
self._scale_g = 0
@@ -129,6 +108,63 @@ def _r_w_reg(self, reg, dat, mask):
self.rb[0] = (self.rb[0] & mask) | dat
self._setreg(reg, self.rb[0])
+ def is_connected(self):
+ """
+ Checks whether the IMU is connected
+
+ :return: True if WHO_AM_I value is correct, otherwise False
+ :rtype: bool
+ """
+ who_am_i = self._getreg(LSM_REG_WHO_AM_I)
+ return who_am_i == 0x6C
+
+ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
+ """
+ Resets the IMU, and restores all registers to their default values
+
+ :param wait_for_reset: Whether to wait for reset to complete
+ :type wait_for_reset: bool
+ :param wait_timeout_ms: Timeout in milliseconds when waiting for reset
+ :type wait_timeout_ms: int
+ :return: False if timeout occurred, otherwise True
+ :rtype: bool
+ """
+ # Set BOOT and SW_RESET bits
+ self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_struct.BOOT = 1
+ self.reg_ctrl3_c_struct.SW_RESET = 1
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
+
+ # Wait for reset to complete, if requested
+ if wait_for_reset:
+ # Loop with timeout
+ t0 = time.ticks_ms()
+ while time.ticks_ms() < (t0 + wait_timeout_ms):
+ # Check if register has returned to default value (0x04)
+ self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ if self.reg_ctrl3_c_byte == 0x04:
+ return True
+ # Timeout occurred
+ return False
+ else:
+ return True
+
+ def _set_bdu(self, bdu = True):
+ """
+ Sets Block Data Update bit
+ """
+ self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_struct.BDU = bdu
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
+
+ def _set_if_inc(self, if_inc = True):
+ """
+ Sets interface increment bit
+ """
+ self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_struct.IF_INC = if_inc
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
+
def _mg(self, reg):
return round(self._int16(self._get2reg(reg)) * 0.061 * self._scale_a_c)
@@ -139,19 +175,19 @@ def _get_gyro_x_rate(self):
"""
Individual axis read for the Gyroscope's X-axis, in mg
"""
- return self._mdps(LSM6DSO_OUTX_L_G) - self.gyro_offsets[0]
+ return self._mdps(LSM_REG_OUTX_L_G) - self.gyro_offsets[0]
def _get_gyro_y_rate(self):
"""
Individual axis read for the Gyroscope's Y-axis, in mg
"""
- return self._mdps(LSM6DSO_OUTY_L_G) - self.gyro_offsets[1]
+ return self._mdps(LSM_REG_OUTY_L_G) - self.gyro_offsets[1]
def _get_gyro_z_rate(self):
"""
Individual axis read for the Gyroscope's Z-axis, in mg
"""
- return self._mdps(LSM6DSO_OUTZ_L_G) - self.gyro_offsets[2]
+ return self._mdps(LSM_REG_OUTZ_L_G) - self.gyro_offsets[2]
def _get_gyro_rates(self):
"""
@@ -182,21 +218,21 @@ def get_acc_x(self):
:return: The current reading for the accelerometer's X-axis, in mg
:rtype: int
"""
- return self._mg(LSM6DSO_OUTX_L_A) - self.acc_offsets[0]
+ return self._mg(LSM_REG_OUTX_L_A) - self.acc_offsets[0]
def get_acc_y(self):
"""
:return: The current reading for the accelerometer's Y-axis, in mg
:rtype: int
"""
- return self._mg(LSM6DSO_OUTY_L_A) - self.acc_offsets[1]
+ return self._mg(LSM_REG_OUTY_L_A) - self.acc_offsets[1]
def get_acc_z(self):
"""
:return: The current reading for the accelerometer's Z-axis, in mg
:rtype: int
"""
- return self._mg(LSM6DSO_OUTZ_L_A) - self.acc_offsets[2]
+ return self._mg(LSM_REG_OUTZ_L_A) - self.acc_offsets[2]
def get_acc_rates(self):
"""
@@ -299,70 +335,88 @@ def temperature(self):
# The LSM6DSO's temperature can be read from the OUT_TEMP_L register
# We use OUT_TEMP_L+1 if OUT_TEMP_L cannot be read
try:
- return self._int16(self._get2reg(LSM6DSO_OUT_TEMP_L))/256 + 25
+ return self._int16(self._get2reg(LSM_REG_OUT_TEMP_L))/256 + 25
except MemoryError:
return self._temperature_irq()
def _temperature_irq(self):
# Helper function for temperature() to read the alternate temperature register
- self._getreg(LSM6DSO_OUT_TEMP_L+1)
+ self._getreg(LSM_REG_OUT_TEMP_L+1)
if self.rb[0] & 0x80:
self.rb[0] -= 256
return self.rb[0] + 25
- def acc_scale(self, dat=None):
+ def acc_scale(self, value=None):
"""
- Set the accelerometer scale. The scale can be '2g', '4g', '8g', or '16g'.
+ Set the accelerometer scale in g. The scale can be:
+ '2g', '4g', '8g', or '16g'
Pass in no parameters to retrieve the current value
"""
- if dat is None:
- return LSM6DSO_SCALEA[self._scale_a]
+ # Get register value
+ self.reg_ctrl1_xl_byte = self._getreg(LSM_REG_CTRL1_XL)
+ # Check if the provided value is in the dictionary
+ if value not in LSM_ACCEL_FS:
+ # Return string representation of this value
+ index = list(LSM_ACCEL_FS.values()).index(self.reg_ctrl1_xl_struct.FS_XL)
+ return list(LSM_ACCEL_FS.keys())[index]
else:
- if type(dat) is str:
- if not dat in LSM6DSO_SCALEA: return
- self._scale_a = LSM6DSO_SCALEA.index(dat)
- self._scale_a_c = int(dat.rstrip('g'))//2
- else:
- return self._r_w_reg(LSM6DSO_CTRL1_XL, self._scale_a<<2, 0xF3)
+ # Set value as requested
+ self.reg_ctrl1_xl_struct.FS_XL = LSM_ACCEL_FS[value]
+ self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte)
- def gyro_scale(self, dat=None):
+ def gyro_scale(self, value=None):
"""
- Set the gyroscope scale. The scale can be '125', '250', '500', '1000', or '2000'.
+ Set the gyroscope scale in dps. The scale can be:
+ '125', '250', '500', '1000', or '2000'
Pass in no parameters to retrieve the current value
"""
- if (dat is None) or (dat == ''):
- return LSM6DSO_SCALEG[self._scale_g]
+ # Get register value
+ self.reg_ctrl2_g_byte = self._getreg(LSM_REG_CTRL2_G)
+ # Check if the provided value is in the dictionary
+ if value not in LSM_GYRO_FS:
+ # Return string representation of this value
+ index = list(LSM_GYRO_FS.values()).index(self.reg_ctrl2_g_struct.FS_G)
+ return list(LSM_GYRO_FS.keys())[index]
else:
- if type(dat) is str:
- if not dat in LSM6DSO_SCALEG: return
- self._scale_g = LSM6DSO_SCALEG.index(dat)
- self._scale_g_c = int(dat)//125
- else: return
- self._r_w_reg(LSM6DSO_CTRL2_G, self._scale_g<<1, 0xF1)
+ # Set value as requested
+ self.reg_ctrl2_g_struct.FS_G = LSM_GYRO_FS[value]
+ self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
- def acc_rate(self, dat=None):
+ def acc_rate(self, value=None):
"""
- Set the accelerometer rate. The rate can be '0', '12.5', '26', '52', '104', '208', '416', '833', '1660', '3330', '6660'.
+ Set the accelerometer rate in Hz. The rate can be:
+ '0Hz', '12.5Hz', '26Hz', '52Hz', '104Hz', '208Hz', '416Hz', '833Hz', '1660Hz', '3330Hz', '6660Hz'
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
+ # Get register value
+ self.reg_ctrl1_xl_byte = self._getreg(LSM_REG_CTRL1_XL)
+ # Check if the provided value is in the dictionary
+ if value not in LSM_ODR:
+ # Return string representation of this value
+ index = list(LSM_ODR.values()).index(self.reg_ctrl1_xl_struct.ODR_XL)
+ return list(LSM_ODR.keys())[index]
else:
- reg_val = LSM6DSO_ODRA.index(dat) << 4
- return self._r_w_reg(LSM6DSO_CTRL1_XL, reg_val, 0x0F)
+ # Set value as requested
+ self.reg_ctrl1_xl_struct.ODR_XL = LSM_ODR[value]
+ self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte)
- def gyro_rate(self, dat=None):
+ def gyro_rate(self, value=None):
"""
- Set the gyroscope rate. The rate can be '0', '12.5', '26', '52', '104', '208', '416', '833', '1660', '3330', '6660'.
+ Set the gyroscope rate in Hz. The rate can be:
+ '0Hz', '12.5Hz', '26Hz', '52Hz', '104Hz', '208Hz', '416Hz', '833Hz', '1660Hz', '3330Hz', '6660Hz'
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
+ # Get register value
+ self.reg_ctrl2_g_byte = self._getreg(LSM_REG_CTRL2_G)
+ # Check if the provided value is in the dictionary
+ if value not in LSM_ODR:
+ # Return string representation of this value
+ index = list(LSM_ODR.values()).index(self.reg_ctrl1_xl_struct.ODR_G)
+ return list(LSM_ODR.keys())[index]
else:
- reg_val = LSM6DSO_ODRG.index(dat) << 4
- return self._r_w_reg(LSM6DSO_CTRL2_G, reg_val, 0x0F)
+ # Set value as requested
+ self.reg_ctrl2_g_struct.ODR_G = LSM_ODR[value]
+ self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl2_g_byte)
def power(self, on:bool=None):
"""
@@ -377,13 +431,13 @@ def power(self, on:bool=None):
else:
self._power = on
if on:
- self._r_w_reg(LSM6DSO_CTRL1_XL, self._power_a, 0x0F)
- self._r_w_reg(LSM6DSO_CTRL2_G, self._power_g, 0x0F)
+ self._r_w_reg(LSM_REG_CTRL1_XL, self._power_a, 0x0F)
+ self._r_w_reg(LSM_REG_CTRL2_G, self._power_g, 0x0F)
else:
- self._power_a = self._getreg(LSM6DSO_CTRL1_XL) & 0xF0
- self._power_g = self._getreg(LSM6DSO_CTRL2_G) & 0xF0
- self._r_w_reg(LSM6DSO_CTRL1_XL, 0, 0x0F)
- self._r_w_reg(LSM6DSO_CTRL2_G, 0, 0x0F)
+ self._power_a = self._getreg(LSM_REG_CTRL1_XL) & 0xF0
+ self._power_g = self._getreg(LSM_REG_CTRL2_G) & 0xF0
+ self._r_w_reg(LSM_REG_CTRL1_XL, 0, 0x0F)
+ self._r_w_reg(LSM_REG_CTRL2_G, 0, 0x0F)
def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:int=5):
"""
diff --git a/XRPLib/imu_defs.py b/XRPLib/imu_defs.py
new file mode 100644
index 0000000..fd7b312
--- /dev/null
+++ b/XRPLib/imu_defs.py
@@ -0,0 +1,76 @@
+from uctypes import BFUINT8, BF_POS, BF_LEN
+from micropython import const
+
+"""
+ Possible I2C addresses
+"""
+LSM_ADDR_PRIMARY = const(0x6B)
+LSM_ADDR_SECONDARY = const(0x6A)
+
+"""
+ Register addresses
+"""
+LSM_REG_WHO_AM_I = const(0x0F)
+LSM_REG_CTRL1_XL = const(0x10)
+LSM_REG_CTRL2_G = const(0x11)
+LSM_REG_CTRL3_C = const(0x12)
+LSM_REG_OUT_TEMP_L = const(0x20)
+LSM_REG_OUT_TEMP_H = const(0x21)
+LSM_REG_OUTX_L_G = const(0x22)
+LSM_REG_OUTY_L_G = const(0x24)
+LSM_REG_OUTZ_L_G = const(0x26)
+LSM_REG_OUTX_L_A = const(0x28)
+LSM_REG_OUTY_L_A = const(0x2A)
+LSM_REG_OUTZ_L_A = const(0x2C)
+
+"""
+ Bit field struct definitions of registers
+"""
+LSM_REG_LAYOUT_CTRL1_XL = {
+ "ODR_XL" : BFUINT8 | 4 << BF_POS | 4 << BF_LEN,
+ "FS_XL" : BFUINT8 | 2 << BF_POS | 2 << BF_LEN,
+ "LPF2_XL_EN" : BFUINT8 | 1 << BF_POS | 1 << BF_LEN,
+}
+LSM_REG_LAYOUT_CTRL2_G = {
+ "ODR_G" : BFUINT8 | 4 << BF_POS | 4 << BF_LEN,
+ "FS_G" : BFUINT8 | 1 << BF_POS | 3 << BF_LEN,
+}
+LSM_REG_LAYOUT_CTRL3_C = {
+ "BOOT" : BFUINT8 | 7 << BF_POS | 1 << BF_LEN,
+ "BDU" : BFUINT8 | 6 << BF_POS | 1 << BF_LEN,
+ "H_LACTIVE" : BFUINT8 | 5 << BF_POS | 1 << BF_LEN,
+ "PP_OD" : BFUINT8 | 4 << BF_POS | 1 << BF_LEN,
+ "SIM" : BFUINT8 | 3 << BF_POS | 1 << BF_LEN,
+ "IF_INC" : BFUINT8 | 2 << BF_POS | 1 << BF_LEN,
+ "SW_RESET" : BFUINT8 | 0 << BF_POS | 1 << BF_LEN,
+}
+
+"""
+ Dictionaries for possible register settings
+"""
+LSM_ODR = {
+ "0Hz" : 0x0,
+ "12.5Hz" : 0x1,
+ "26Hz" : 0x2,
+ "52Hz" : 0x3,
+ "104Hz" : 0x4,
+ "208Hz" : 0x5,
+ "416Hz" : 0x6,
+ "833Hz" : 0x7,
+ "1660Hz" : 0x8,
+ "3330Hz" : 0x9,
+ "6660Hz" : 0xA,
+}
+LSM_ACCEL_FS = {
+ "2g" : 0x0,
+ "4g" : 0x2,
+ "8g" : 0x3,
+ "16g" : 0x1,
+}
+LSM_GYRO_FS = {
+ "125dps" : 0x1,
+ "250dps" : 0x0,
+ "500dps" : 0x2,
+ "1000dps" : 0x4,
+ "2000dps" : 0x6,
+}
\ No newline at end of file
From 3e51da605ef4147cdfebf00e32aed54162cc5656 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 15:29:27 -0600
Subject: [PATCH 09/22] Fix typos
Add 'Hz' and 'dps' to rate function calls
Fix register assignment in gyro_rate()
---
XRPLib/imu.py | 8 ++++----
1 file changed, 4 insertions(+), 4 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 7151eb5..8493095 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -65,9 +65,9 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self._scale_a_c = 1
self._scale_g_c = 1
self.acc_scale('16g')
- self.gyro_scale('2000')
- self.acc_rate('208')
- self.gyro_rate('208')
+ self.gyro_scale('2000dps')
+ self.acc_rate('208Hz')
+ self.gyro_rate('208Hz')
self.gyro_offsets = [0,0,0]
self.acc_offsets = [0,0,0]
@@ -416,7 +416,7 @@ def gyro_rate(self, value=None):
else:
# Set value as requested
self.reg_ctrl2_g_struct.ODR_G = LSM_ODR[value]
- self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl2_g_byte)
+ self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
def power(self, on:bool=None):
"""
From eb98e570317dd677a33fa17c55ac2f24cc85b969 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 15:39:22 -0600
Subject: [PATCH 10/22] Change IMU timer frequency
Timer now uses frequency instead of period
Frequency is now calculated when gyro_rate() is called
Remove update_time parameter from calibrate()
Add _start_timer() and stop_timer()
---
XRPLib/imu.py | 28 +++++++++++++++++-----------
1 file changed, 17 insertions(+), 11 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 8493095..e166c58 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -72,7 +72,7 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self.gyro_offsets = [0,0,0]
self.acc_offsets = [0,0,0]
- self.update_time = 0.005
+ self.timer_frequency = 208
self.gyro_pitch_bias = 0
self.adjusted_pitch = 0
@@ -418,6 +418,10 @@ def gyro_rate(self, value=None):
self.reg_ctrl2_g_struct.ODR_G = LSM_ODR[value]
self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
+ # Update timer frequency
+ self.timer_frequency = int(value.rstrip('Hz'))
+ self._start_timer()
+
def power(self, on:bool=None):
"""
Turn the LSM6DSO on or off.
@@ -439,7 +443,7 @@ def power(self, on:bool=None):
self._r_w_reg(LSM_REG_CTRL1_XL, 0, 0x0F)
self._r_w_reg(LSM_REG_CTRL2_G, 0, 0x0F)
- def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:int=5):
+ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2):
"""
Collect readings for [calibration_time] seconds and calibrate the IMU based on those readings
Do not move the robot during this time
@@ -449,10 +453,8 @@ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:
:type calibration_time: float
:param vertical_axis: The axis that is vertical. 0 for X, 1 for Y, 2 for Z
:type vertical_axis: int
- :param update_time: The time in milliseconds between each update of the IMU
- :type update_time: int
"""
- self.update_timer.deinit()
+ self._stop_timer()
self.acc_offsets = [0,0,0]
self.gyro_offsets = [0,0,0]
avg_vals = [[0,0,0],[0,0,0]]
@@ -472,7 +474,7 @@ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:
avg_vals[1][2] += cur_vals[1][2]
# Increment counter and wait for next loop
num_vals += 1
- time.sleep(update_time / 1000)
+ time.sleep(1 / self.timer_frequency)
# Compute averages
avg_vals[0][0] /= num_vals
@@ -486,16 +488,20 @@ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2, update_time:
self.acc_offsets = avg_vals[0]
self.gyro_offsets = avg_vals[1]
- self.update_timer.init(period=update_time, callback=lambda t:self._update_imu_readings())
- self.update_time = update_time/1000
+ self._start_timer()
+ def _start_timer(self):
+ self.update_timer.init(freq=self.timer_frequency, callback=lambda t:self._update_imu_readings())
+
+ def _stop_timer(self):
+ self.update_timer.deinit()
def _update_imu_readings(self):
# Called every tick through a callback timer
- delta_pitch = self._get_gyro_x_rate()*self.update_time / 1000
- delta_roll = self._get_gyro_y_rate()*self.update_time / 1000
- delta_yaw = self._get_gyro_z_rate()*self.update_time / 1000
+ delta_pitch = self._get_gyro_x_rate() / 1000 / self.timer_frequency
+ delta_roll = self._get_gyro_y_rate() / 1000 / self.timer_frequency
+ delta_yaw = self._get_gyro_z_rate() / 1000 / self.timer_frequency
state = disable_irq()
self.running_pitch += delta_pitch
From 974edd9b013247606accce0bbbb37549fafdb4c1 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 16:09:31 -0600
Subject: [PATCH 11/22] Clean up imu.py
Remove unused member variables
Add comments to __init__()
Add constants for mg and mpds per least significant bit
---
XRPLib/imu.py | 31 ++++++++++++++++---------------
XRPLib/imu_defs.py | 8 +++++++-
2 files changed, 23 insertions(+), 16 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index e166c58..8f2c724 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -29,12 +29,18 @@ def get_default_imu(cls):
return cls._DEFAULT_IMU_INSTANCE
def __init__(self, scl_pin: int, sda_pin: int, addr):
+ # I2C values
self.i2c = I2C(id=1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=400000)
self.addr = addr
+
+ # Transmit and recieve buffers
self.tb = bytearray(1)
self.rb = bytearray(1)
- self.oneshot = False
+
+ # Vector of IMU measurements
self.irq_v = [[0, 0, 0], [0, 0, 0]]
+
+ # Power control
self._power = True
self._power_a = 0x10
self._power_g = 0x10
@@ -59,29 +65,24 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
# Enable block data update
self._set_bdu()
- # scale=2G
- self._scale_a = 0
- self._scale_g = 0
- self._scale_a_c = 1
- self._scale_g_c = 1
+ # Set default scale for each sensor
self.acc_scale('16g')
self.gyro_scale('2000dps')
+
+ # Set default rate for each sensor
self.acc_rate('208Hz')
self.gyro_rate('208Hz')
+ # Initialize offsets to zero
self.gyro_offsets = [0,0,0]
self.acc_offsets = [0,0,0]
- self.timer_frequency = 208
- self.gyro_pitch_bias = 0
- self.adjusted_pitch = 0
-
- self.gyro_pitch_running_total = 0
-
+ # Initialize integrators to zero
self.running_pitch = 0
self.running_yaw = 0
self.running_roll = 0
+ # Create timer
self.update_timer = Timer(-1)
@@ -159,17 +160,17 @@ def _set_bdu(self, bdu = True):
def _set_if_inc(self, if_inc = True):
"""
- Sets interface increment bit
+ Sets InterFace INCrement bit
"""
self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
self.reg_ctrl3_c_struct.IF_INC = if_inc
self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
def _mg(self, reg):
- return round(self._int16(self._get2reg(reg)) * 0.061 * self._scale_a_c)
+ return round(self._int16(self._get2reg(reg)) * LSM_MG_PER_LSB)
def _mdps(self, reg):
- return round(self._int16(self._get2reg(reg)) * 4.375 * self._scale_g_c)
+ return round(self._int16(self._get2reg(reg)) * LSM_MDPS_PER_LSB)
def _get_gyro_x_rate(self):
"""
diff --git a/XRPLib/imu_defs.py b/XRPLib/imu_defs.py
index fd7b312..3d03c86 100644
--- a/XRPLib/imu_defs.py
+++ b/XRPLib/imu_defs.py
@@ -73,4 +73,10 @@
"500dps" : 0x2,
"1000dps" : 0x4,
"2000dps" : 0x6,
-}
\ No newline at end of file
+}
+
+"""
+ Other contants
+"""
+LSM_MG_PER_LSB = 0.061
+LSM_MDPS_PER_LSB = 4.375
\ No newline at end of file
From b857209e4eb905da1a299af85646e861dd6e9d27 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 16:12:17 -0600
Subject: [PATCH 12/22] Remove power() from imu.py
It has some edge cases with the rate functions. We could handle them, but that's more effort than just removing power(). It's not really needed for the XRP, but users can still reduce power consumption by sending '0Hz' to the rate functions.
---
XRPLib/imu.py | 26 --------------------------
1 file changed, 26 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 8f2c724..735e1f7 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -40,11 +40,6 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
# Vector of IMU measurements
self.irq_v = [[0, 0, 0], [0, 0, 0]]
- # Power control
- self._power = True
- self._power_a = 0x10
- self._power_g = 0x10
-
# Copies of registers. Bytes and structs share the same memory
# addresses, so changing one changes the other
self.reg_ctrl1_xl_byte = bytearray(1)
@@ -423,27 +418,6 @@ def gyro_rate(self, value=None):
self.timer_frequency = int(value.rstrip('Hz'))
self._start_timer()
- def power(self, on:bool=None):
- """
- Turn the LSM6DSO on or off.
- Pass in no parameters to retrieve the current value
-
- :param on: Whether to turn the LSM6DSO on or off, or None
- :type on: bool (or None)
- """
- if on is None:
- return self._power
- else:
- self._power = on
- if on:
- self._r_w_reg(LSM_REG_CTRL1_XL, self._power_a, 0x0F)
- self._r_w_reg(LSM_REG_CTRL2_G, self._power_g, 0x0F)
- else:
- self._power_a = self._getreg(LSM_REG_CTRL1_XL) & 0xF0
- self._power_g = self._getreg(LSM_REG_CTRL2_G) & 0xF0
- self._r_w_reg(LSM_REG_CTRL1_XL, 0, 0x0F)
- self._r_w_reg(LSM_REG_CTRL2_G, 0, 0x0F)
-
def calibrate(self, calibration_time:float=1, vertical_axis:int= 2):
"""
Collect readings for [calibration_time] seconds and calibrate the IMU based on those readings
From c0362011a3a53acc8096696d04153dec25a8b286 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 21:52:20 -0600
Subject: [PATCH 13/22] Rename bitfield "struct"s to "bits"
Bits and bytes have better naming parity between each other
---
XRPLib/imu.py | 30 +++++++++++++++---------------
1 file changed, 15 insertions(+), 15 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 735e1f7..17363c8 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -45,9 +45,9 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self.reg_ctrl1_xl_byte = bytearray(1)
self.reg_ctrl2_g_byte = bytearray(1)
self.reg_ctrl3_c_byte = bytearray(1)
- self.reg_ctrl1_xl_struct = struct(addressof(self.reg_ctrl1_xl_byte), LSM_REG_LAYOUT_CTRL1_XL)
- self.reg_ctrl2_g_struct = struct(addressof(self.reg_ctrl2_g_byte), LSM_REG_LAYOUT_CTRL2_G)
- self.reg_ctrl3_c_struct = struct(addressof(self.reg_ctrl3_c_byte), LSM_REG_LAYOUT_CTRL3_C)
+ self.reg_ctrl1_xl_bits = struct(addressof(self.reg_ctrl1_xl_byte), LSM_REG_LAYOUT_CTRL1_XL)
+ self.reg_ctrl2_g_bits = struct(addressof(self.reg_ctrl2_g_byte), LSM_REG_LAYOUT_CTRL2_G)
+ self.reg_ctrl3_c_bits = struct(addressof(self.reg_ctrl3_c_byte), LSM_REG_LAYOUT_CTRL3_C)
# Check if the IMU is connected
if not self.is_connected():
@@ -127,8 +127,8 @@ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
"""
# Set BOOT and SW_RESET bits
self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
- self.reg_ctrl3_c_struct.BOOT = 1
- self.reg_ctrl3_c_struct.SW_RESET = 1
+ self.reg_ctrl3_c_bits.BOOT = 1
+ self.reg_ctrl3_c_bits.SW_RESET = 1
self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
# Wait for reset to complete, if requested
@@ -150,7 +150,7 @@ def _set_bdu(self, bdu = True):
Sets Block Data Update bit
"""
self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
- self.reg_ctrl3_c_struct.BDU = bdu
+ self.reg_ctrl3_c_bits.BDU = bdu
self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
def _set_if_inc(self, if_inc = True):
@@ -158,7 +158,7 @@ def _set_if_inc(self, if_inc = True):
Sets InterFace INCrement bit
"""
self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
- self.reg_ctrl3_c_struct.IF_INC = if_inc
+ self.reg_ctrl3_c_bits.IF_INC = if_inc
self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
def _mg(self, reg):
@@ -353,11 +353,11 @@ def acc_scale(self, value=None):
# Check if the provided value is in the dictionary
if value not in LSM_ACCEL_FS:
# Return string representation of this value
- index = list(LSM_ACCEL_FS.values()).index(self.reg_ctrl1_xl_struct.FS_XL)
+ index = list(LSM_ACCEL_FS.values()).index(self.reg_ctrl1_xl_bits.FS_XL)
return list(LSM_ACCEL_FS.keys())[index]
else:
# Set value as requested
- self.reg_ctrl1_xl_struct.FS_XL = LSM_ACCEL_FS[value]
+ self.reg_ctrl1_xl_bits.FS_XL = LSM_ACCEL_FS[value]
self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte)
def gyro_scale(self, value=None):
@@ -371,11 +371,11 @@ def gyro_scale(self, value=None):
# Check if the provided value is in the dictionary
if value not in LSM_GYRO_FS:
# Return string representation of this value
- index = list(LSM_GYRO_FS.values()).index(self.reg_ctrl2_g_struct.FS_G)
+ index = list(LSM_GYRO_FS.values()).index(self.reg_ctrl2_g_bits.FS_G)
return list(LSM_GYRO_FS.keys())[index]
else:
# Set value as requested
- self.reg_ctrl2_g_struct.FS_G = LSM_GYRO_FS[value]
+ self.reg_ctrl2_g_bits.FS_G = LSM_GYRO_FS[value]
self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
def acc_rate(self, value=None):
@@ -389,11 +389,11 @@ def acc_rate(self, value=None):
# Check if the provided value is in the dictionary
if value not in LSM_ODR:
# Return string representation of this value
- index = list(LSM_ODR.values()).index(self.reg_ctrl1_xl_struct.ODR_XL)
+ index = list(LSM_ODR.values()).index(self.reg_ctrl1_xl_bits.ODR_XL)
return list(LSM_ODR.keys())[index]
else:
# Set value as requested
- self.reg_ctrl1_xl_struct.ODR_XL = LSM_ODR[value]
+ self.reg_ctrl1_xl_bits.ODR_XL = LSM_ODR[value]
self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte)
def gyro_rate(self, value=None):
@@ -407,11 +407,11 @@ def gyro_rate(self, value=None):
# Check if the provided value is in the dictionary
if value not in LSM_ODR:
# Return string representation of this value
- index = list(LSM_ODR.values()).index(self.reg_ctrl1_xl_struct.ODR_G)
+ index = list(LSM_ODR.values()).index(self.reg_ctrl1_xl_bits.ODR_G)
return list(LSM_ODR.keys())[index]
else:
# Set value as requested
- self.reg_ctrl2_g_struct.ODR_G = LSM_ODR[value]
+ self.reg_ctrl2_g_bits.ODR_G = LSM_ODR[value]
self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
# Update timer frequency
From bfce0fa4d49435b5aac8d213e97767950f8f9da2 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Sun, 16 Jul 2023 22:52:06 -0600
Subject: [PATCH 14/22] Burst read IMU data registers
Burst reading speeds up data transmission a lot, up to 3x faster than reading single bytes when reading lots of registers (eg. reading 12 bytes at 400kHz should be almost 500us faster)
Also best to minimize the amount of time spent hogging the I2C bus in case something else needs it
---
XRPLib/imu.py | 92 +++++++++++++++++++++++++++++++++++++++------------
1 file changed, 70 insertions(+), 22 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 17363c8..fdcec10 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -96,6 +96,11 @@ def _getreg(self, reg):
self.i2c.readfrom_mem_into(self.addr, reg, self.rb)
return self.rb[0]
+ def _getregs(self, reg, num_bytes):
+ rx_buf = bytearray(num_bytes)
+ self.i2c.readfrom_mem_into(self.addr, reg, rx_buf)
+ return rx_buf
+
def _get2reg(self, reg):
return self._getreg(reg) + self._getreg(reg+1) * 256
@@ -161,38 +166,55 @@ def _set_if_inc(self, if_inc = True):
self.reg_ctrl3_c_bits.IF_INC = if_inc
self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
- def _mg(self, reg):
- return round(self._int16(self._get2reg(reg)) * LSM_MG_PER_LSB)
+ def _raw_to_mg(self, raw):
+ return self._int16((raw[1] << 8) | raw[0]) * LSM_MG_PER_LSB
- def _mdps(self, reg):
- return round(self._int16(self._get2reg(reg)) * LSM_MDPS_PER_LSB)
+ def _raw_to_mdps(self, raw):
+ return self._int16((raw[1] << 8) | raw[0]) * LSM_MDPS_PER_LSB
def _get_gyro_x_rate(self):
"""
Individual axis read for the Gyroscope's X-axis, in mg
"""
- return self._mdps(LSM_REG_OUTX_L_G) - self.gyro_offsets[0]
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 2)
+
+ # Convert raw data to mdps
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
def _get_gyro_y_rate(self):
"""
Individual axis read for the Gyroscope's Y-axis, in mg
"""
- return self._mdps(LSM_REG_OUTY_L_G) - self.gyro_offsets[1]
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTY_L_G, 2)
+
+ # Convert raw data to mdps
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[1]
def _get_gyro_z_rate(self):
"""
Individual axis read for the Gyroscope's Z-axis, in mg
"""
- return self._mdps(LSM_REG_OUTZ_L_G) - self.gyro_offsets[2]
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTZ_L_G, 2)
+
+ # Convert raw data to mdps
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[2]
def _get_gyro_rates(self):
"""
Retrieves the array of readings from the Gyroscope, in mdps
The order of the values is x, y, z.
"""
- self.irq_v[1][0] = self._get_gyro_x_rate()
- self.irq_v[1][1] = self._get_gyro_y_rate()
- self.irq_v[1][2] = self._get_gyro_z_rate()
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 6)
+
+ # Convert raw data to mdps
+ self.irq_v[1][0] = self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
+ self.irq_v[1][1] = self._raw_to_mdps(raw_bytes[2:4]) - self.gyro_offsets[1]
+ self.irq_v[1][2] = self._raw_to_mdps(raw_bytes[4:6]) - self.gyro_offsets[2]
+
return self.irq_v[1]
def _get_acc_gyro_rates(self):
@@ -201,8 +223,17 @@ def _get_acc_gyro_rates(self):
The first row is the acceleration values, the second row is the gyro values.
The order of the values is x, y, z.
"""
- self.get_acc_rates()
- self._get_gyro_rates()
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 12)
+
+ # Convert raw data to mg's and mdps
+ self.irq_v[0][0] = self._raw_to_mg(raw_bytes[6:8]) - self.acc_offsets[0]
+ self.irq_v[0][1] = self._raw_to_mg(raw_bytes[8:10]) - self.acc_offsets[1]
+ self.irq_v[0][2] = self._raw_to_mg(raw_bytes[10:12]) - self.acc_offsets[2]
+ self.irq_v[1][0] = self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
+ self.irq_v[1][1] = self._raw_to_mdps(raw_bytes[2:4]) - self.gyro_offsets[1]
+ self.irq_v[1][2] = self._raw_to_mdps(raw_bytes[4:6]) - self.gyro_offsets[2]
+
return self.irq_v
"""
@@ -214,30 +245,47 @@ def get_acc_x(self):
:return: The current reading for the accelerometer's X-axis, in mg
:rtype: int
"""
- return self._mg(LSM_REG_OUTX_L_A) - self.acc_offsets[0]
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_A, 2)
+
+ # Convert raw data to mg's
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
def get_acc_y(self):
"""
:return: The current reading for the accelerometer's Y-axis, in mg
:rtype: int
"""
- return self._mg(LSM_REG_OUTY_L_A) - self.acc_offsets[1]
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTY_L_A, 2)
+
+ # Convert raw data to mg's
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[1]
def get_acc_z(self):
"""
:return: The current reading for the accelerometer's Z-axis, in mg
:rtype: int
"""
- return self._mg(LSM_REG_OUTZ_L_A) - self.acc_offsets[2]
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTZ_L_A, 2)
+
+ # Convert raw data to mg's
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[2]
def get_acc_rates(self):
"""
:return: the list of readings from the Accelerometer, in mg. The order of the values is x, y, z.
:rtype: list<int>
"""
- self.irq_v[0][0] = self.get_acc_x()
- self.irq_v[0][1] = self.get_acc_y()
- self.irq_v[0][2] = self.get_acc_z()
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_A, 6)
+
+ # Convert raw data to mg's
+ self.irq_v[0][0] = self._raw_to_mg(raw_bytes[0:2]) - self.acc_offsets[0]
+ self.irq_v[0][1] = self._raw_to_mg(raw_bytes[2:4]) - self.acc_offsets[1]
+ self.irq_v[0][2] = self._raw_to_mg(raw_bytes[4:6]) - self.acc_offsets[2]
+
return self.irq_v[0]
def get_pitch(self):
@@ -473,10 +521,10 @@ def _stop_timer(self):
def _update_imu_readings(self):
# Called every tick through a callback timer
-
- delta_pitch = self._get_gyro_x_rate() / 1000 / self.timer_frequency
- delta_roll = self._get_gyro_y_rate() / 1000 / self.timer_frequency
- delta_yaw = self._get_gyro_z_rate() / 1000 / self.timer_frequency
+ self._get_gyro_rates()
+ delta_pitch = self.irq_v[1][0] / 1000 / self.timer_frequency
+ delta_roll = self.irq_v[1][1] / 1000 / self.timer_frequency
+ delta_yaw = self.irq_v[1][2] / 1000 / self.timer_frequency
state = disable_irq()
self.running_pitch += delta_pitch
From 6a47c4b9505bb1302662950b19a37ab9cca9c618 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Mon, 17 Jul 2023 10:02:06 -0600
Subject: [PATCH 15/22] Move IMU timer definition
Needs to be defined before calling gyro_rate()
---
XRPLib/imu.py | 7 +++----
1 file changed, 3 insertions(+), 4 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index fdcec10..e48c6ae 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -49,6 +49,9 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self.reg_ctrl2_g_bits = struct(addressof(self.reg_ctrl2_g_byte), LSM_REG_LAYOUT_CTRL2_G)
self.reg_ctrl3_c_bits = struct(addressof(self.reg_ctrl3_c_byte), LSM_REG_LAYOUT_CTRL3_C)
+ # Create timer
+ self.update_timer = Timer(-1)
+
# Check if the IMU is connected
if not self.is_connected():
# TODO - do somehting intelligent here
@@ -77,10 +80,6 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self.running_yaw = 0
self.running_roll = 0
- # Create timer
- self.update_timer = Timer(-1)
-
-
"""
The following are private helper methods to read and write registers, as well as to convert the read values to the correct unit.
"""
From 94f8cbc198c0f96eab1f5eb4da2691000f701f0e Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Mon, 17 Jul 2023 11:05:46 -0600
Subject: [PATCH 16/22] Fix accelerometer typos
Forgot to change from gyro to accel when copying, oops!
---
XRPLib/imu.py | 6 +++---
1 file changed, 3 insertions(+), 3 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index e48c6ae..42429ee 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -248,7 +248,7 @@ def get_acc_x(self):
raw_bytes = self._getregs(LSM_REG_OUTX_L_A, 2)
# Convert raw data to mg's
- return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
+ return self._raw_to_mg(raw_bytes[0:2]) - self.acc_offsets[0]
def get_acc_y(self):
"""
@@ -259,7 +259,7 @@ def get_acc_y(self):
raw_bytes = self._getregs(LSM_REG_OUTY_L_A, 2)
# Convert raw data to mg's
- return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[1]
+ return self._raw_to_mg(raw_bytes[0:2]) - self.acc_offsets[1]
def get_acc_z(self):
"""
@@ -270,7 +270,7 @@ def get_acc_z(self):
raw_bytes = self._getregs(LSM_REG_OUTZ_L_A, 2)
# Convert raw data to mg's
- return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[2]
+ return self._raw_to_mg(raw_bytes[0:2]) - self.acc_offsets[2]
def get_acc_rates(self):
"""
From 7445655eba6981f599d2142539c128ec3f973db9 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Mon, 17 Jul 2023 11:06:39 -0600
Subject: [PATCH 17/22] Fix bytearray indexing
Need to index into bytearray rather than reassigning the variable
---
XRPLib/imu.py | 32 ++++++++++++++++----------------
1 file changed, 16 insertions(+), 16 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 42429ee..dade0be 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -130,10 +130,10 @@ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
:rtype: bool
"""
# Set BOOT and SW_RESET bits
- self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
self.reg_ctrl3_c_bits.BOOT = 1
self.reg_ctrl3_c_bits.SW_RESET = 1
- self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte[0])
# Wait for reset to complete, if requested
if wait_for_reset:
@@ -141,8 +141,8 @@ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
t0 = time.ticks_ms()
while time.ticks_ms() < (t0 + wait_timeout_ms):
# Check if register has returned to default value (0x04)
- self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
- if self.reg_ctrl3_c_byte == 0x04:
+ self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
+ if self.reg_ctrl3_c_byte[0] == 0x04:
return True
# Timeout occurred
return False
@@ -153,17 +153,17 @@ def _set_bdu(self, bdu = True):
"""
Sets Block Data Update bit
"""
- self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
self.reg_ctrl3_c_bits.BDU = bdu
- self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte[0])
def _set_if_inc(self, if_inc = True):
"""
Sets InterFace INCrement bit
"""
- self.reg_ctrl3_c_byte = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
self.reg_ctrl3_c_bits.IF_INC = if_inc
- self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte)
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte[0])
def _raw_to_mg(self, raw):
return self._int16((raw[1] << 8) | raw[0]) * LSM_MG_PER_LSB
@@ -396,7 +396,7 @@ def acc_scale(self, value=None):
Pass in no parameters to retrieve the current value
"""
# Get register value
- self.reg_ctrl1_xl_byte = self._getreg(LSM_REG_CTRL1_XL)
+ self.reg_ctrl1_xl_byte[0] = self._getreg(LSM_REG_CTRL1_XL)
# Check if the provided value is in the dictionary
if value not in LSM_ACCEL_FS:
# Return string representation of this value
@@ -405,7 +405,7 @@ def acc_scale(self, value=None):
else:
# Set value as requested
self.reg_ctrl1_xl_bits.FS_XL = LSM_ACCEL_FS[value]
- self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte)
+ self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte[0])
def gyro_scale(self, value=None):
"""
@@ -414,7 +414,7 @@ def gyro_scale(self, value=None):
Pass in no parameters to retrieve the current value
"""
# Get register value
- self.reg_ctrl2_g_byte = self._getreg(LSM_REG_CTRL2_G)
+ self.reg_ctrl2_g_byte[0] = self._getreg(LSM_REG_CTRL2_G)
# Check if the provided value is in the dictionary
if value not in LSM_GYRO_FS:
# Return string representation of this value
@@ -423,7 +423,7 @@ def gyro_scale(self, value=None):
else:
# Set value as requested
self.reg_ctrl2_g_bits.FS_G = LSM_GYRO_FS[value]
- self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
+ self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte[0])
def acc_rate(self, value=None):
"""
@@ -432,7 +432,7 @@ def acc_rate(self, value=None):
Pass in no parameters to retrieve the current value
"""
# Get register value
- self.reg_ctrl1_xl_byte = self._getreg(LSM_REG_CTRL1_XL)
+ self.reg_ctrl1_xl_byte[0] = self._getreg(LSM_REG_CTRL1_XL)
# Check if the provided value is in the dictionary
if value not in LSM_ODR:
# Return string representation of this value
@@ -441,7 +441,7 @@ def acc_rate(self, value=None):
else:
# Set value as requested
self.reg_ctrl1_xl_bits.ODR_XL = LSM_ODR[value]
- self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte)
+ self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte[0])
def gyro_rate(self, value=None):
"""
@@ -450,7 +450,7 @@ def gyro_rate(self, value=None):
Pass in no parameters to retrieve the current value
"""
# Get register value
- self.reg_ctrl2_g_byte = self._getreg(LSM_REG_CTRL2_G)
+ self.reg_ctrl2_g_byte[0] = self._getreg(LSM_REG_CTRL2_G)
# Check if the provided value is in the dictionary
if value not in LSM_ODR:
# Return string representation of this value
@@ -459,7 +459,7 @@ def gyro_rate(self, value=None):
else:
# Set value as requested
self.reg_ctrl2_g_bits.ODR_G = LSM_ODR[value]
- self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte)
+ self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte[0])
# Update timer frequency
self.timer_frequency = int(value.rstrip('Hz'))
From 6cf4ee8e3f4ecace35f9e2adecc66c4c371b4806 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Mon, 17 Jul 2023 11:18:00 -0600
Subject: [PATCH 18/22] Fix IMU scaling at different ranges
mg/LSB and dps/LSB are defined for the minimum range of each sensor, so need to scale when using different ranges
---
XRPLib/imu.py | 12 ++++++++++--
XRPLib/imu_defs.py | 4 ++--
2 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index dade0be..2ef9559 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -39,6 +39,10 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
# Vector of IMU measurements
self.irq_v = [[0, 0, 0], [0, 0, 0]]
+
+ # Scale factors when ranges are changed
+ self._acc_scale_factor = 1
+ self._gyro_scale_factor = 1
# Copies of registers. Bytes and structs share the same memory
# addresses, so changing one changes the other
@@ -166,10 +170,10 @@ def _set_if_inc(self, if_inc = True):
self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte[0])
def _raw_to_mg(self, raw):
- return self._int16((raw[1] << 8) | raw[0]) * LSM_MG_PER_LSB
+ return self._int16((raw[1] << 8) | raw[0]) * LSM_MG_PER_LSB_2G * self._acc_scale_factor
def _raw_to_mdps(self, raw):
- return self._int16((raw[1] << 8) | raw[0]) * LSM_MDPS_PER_LSB
+ return self._int16((raw[1] << 8) | raw[0]) * LSM_MDPS_PER_LSB_125DPS * self._gyro_scale_factor
def _get_gyro_x_rate(self):
"""
@@ -406,6 +410,8 @@ def acc_scale(self, value=None):
# Set value as requested
self.reg_ctrl1_xl_bits.FS_XL = LSM_ACCEL_FS[value]
self._setreg(LSM_REG_CTRL1_XL, self.reg_ctrl1_xl_byte[0])
+ # Update scale factor for converting raw data
+ self._acc_scale_factor = int(value.rstrip('g')) // 2
def gyro_scale(self, value=None):
"""
@@ -424,6 +430,8 @@ def gyro_scale(self, value=None):
# Set value as requested
self.reg_ctrl2_g_bits.FS_G = LSM_GYRO_FS[value]
self._setreg(LSM_REG_CTRL2_G, self.reg_ctrl2_g_byte[0])
+ # Update scale factor for converting raw data
+ self._gyro_scale_factor = int(value.rstrip('dps')) // 125
def acc_rate(self, value=None):
"""
diff --git a/XRPLib/imu_defs.py b/XRPLib/imu_defs.py
index 3d03c86..7ccdcb3 100644
--- a/XRPLib/imu_defs.py
+++ b/XRPLib/imu_defs.py
@@ -78,5 +78,5 @@
"""
Other contants
"""
-LSM_MG_PER_LSB = 0.061
-LSM_MDPS_PER_LSB = 4.375
\ No newline at end of file
+LSM_MG_PER_LSB_2G = 0.061
+LSM_MDPS_PER_LSB_125DPS = 4.375
\ No newline at end of file
From f4cfb94e1e6fae2701d4308e099ae419a7d8604a Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Mon, 17 Jul 2023 17:57:43 -0600
Subject: [PATCH 19/22] Move is_connected() and reset() to public section
---
XRPLib/imu.py | 82 +++++++++++++++++++++++++--------------------------
1 file changed, 41 insertions(+), 41 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 2ef9559..d9a15c9 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -112,47 +112,6 @@ def _r_w_reg(self, reg, dat, mask):
self.rb[0] = (self.rb[0] & mask) | dat
self._setreg(reg, self.rb[0])
- def is_connected(self):
- """
- Checks whether the IMU is connected
-
- :return: True if WHO_AM_I value is correct, otherwise False
- :rtype: bool
- """
- who_am_i = self._getreg(LSM_REG_WHO_AM_I)
- return who_am_i == 0x6C
-
- def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
- """
- Resets the IMU, and restores all registers to their default values
-
- :param wait_for_reset: Whether to wait for reset to complete
- :type wait_for_reset: bool
- :param wait_timeout_ms: Timeout in milliseconds when waiting for reset
- :type wait_timeout_ms: int
- :return: False if timeout occurred, otherwise True
- :rtype: bool
- """
- # Set BOOT and SW_RESET bits
- self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
- self.reg_ctrl3_c_bits.BOOT = 1
- self.reg_ctrl3_c_bits.SW_RESET = 1
- self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte[0])
-
- # Wait for reset to complete, if requested
- if wait_for_reset:
- # Loop with timeout
- t0 = time.ticks_ms()
- while time.ticks_ms() < (t0 + wait_timeout_ms):
- # Check if register has returned to default value (0x04)
- self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
- if self.reg_ctrl3_c_byte[0] == 0x04:
- return True
- # Timeout occurred
- return False
- else:
- return True
-
def _set_bdu(self, bdu = True):
"""
Sets Block Data Update bit
@@ -243,6 +202,47 @@ def _get_acc_gyro_rates(self):
Public facing API Methods
"""
+ def is_connected(self):
+ """
+ Checks whether the IMU is connected
+
+ :return: True if WHO_AM_I value is correct, otherwise False
+ :rtype: bool
+ """
+ who_am_i = self._getreg(LSM_REG_WHO_AM_I)
+ return who_am_i == 0x6C
+
+ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
+ """
+ Resets the IMU, and restores all registers to their default values
+
+ :param wait_for_reset: Whether to wait for reset to complete
+ :type wait_for_reset: bool
+ :param wait_timeout_ms: Timeout in milliseconds when waiting for reset
+ :type wait_timeout_ms: int
+ :return: False if timeout occurred, otherwise True
+ :rtype: bool
+ """
+ # Set BOOT and SW_RESET bits
+ self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
+ self.reg_ctrl3_c_bits.BOOT = 1
+ self.reg_ctrl3_c_bits.SW_RESET = 1
+ self._setreg(LSM_REG_CTRL3_C, self.reg_ctrl3_c_byte[0])
+
+ # Wait for reset to complete, if requested
+ if wait_for_reset:
+ # Loop with timeout
+ t0 = time.ticks_ms()
+ while time.ticks_ms() < (t0 + wait_timeout_ms):
+ # Check if register has returned to default value (0x04)
+ self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
+ if self.reg_ctrl3_c_byte[0] == 0x04:
+ return True
+ # Timeout occurred
+ return False
+ else:
+ return True
+
def get_acc_x(self):
"""
:return: The current reading for the accelerometer's X-axis, in mg
From 7c2f19a5aa9a656a6b340f7769eca6824d9825c7 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Mon, 17 Jul 2023 18:13:13 -0600
Subject: [PATCH 20/22] Reset member IMU variables when reset() is called
Also add _reset_member_variables() to reduce duplicate code in reset() and __init__()
---
XRPLib/imu.py | 36 +++++++++++++++++++++++-------------
1 file changed, 23 insertions(+), 13 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index d9a15c9..149d6fc 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -33,17 +33,13 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self.i2c = I2C(id=1, scl=Pin(scl_pin), sda=Pin(sda_pin), freq=400000)
self.addr = addr
+ # Initialize member variables
+ self._reset_member_variables()
+
# Transmit and recieve buffers
self.tb = bytearray(1)
self.rb = bytearray(1)
- # Vector of IMU measurements
- self.irq_v = [[0, 0, 0], [0, 0, 0]]
-
- # Scale factors when ranges are changed
- self._acc_scale_factor = 1
- self._gyro_scale_factor = 1
-
# Copies of registers. Bytes and structs share the same memory
# addresses, so changing one changes the other
self.reg_ctrl1_xl_byte = bytearray(1)
@@ -75,19 +71,27 @@ def __init__(self, scl_pin: int, sda_pin: int, addr):
self.acc_rate('208Hz')
self.gyro_rate('208Hz')
- # Initialize offsets to zero
+ """
+ The following are private helper methods to read and write registers, as well as to convert the read values to the correct unit.
+ """
+
+ def _reset_member_variables(self):
+ # Vector of IMU measurements
+ self.irq_v = [[0, 0, 0], [0, 0, 0]]
+
+ # Sensor offsets
self.gyro_offsets = [0,0,0]
self.acc_offsets = [0,0,0]
- # Initialize integrators to zero
+ # Scale factors when ranges are changed
+ self._acc_scale_factor = 1
+ self._gyro_scale_factor = 1
+
+ # Angle integrators
self.running_pitch = 0
self.running_yaw = 0
self.running_roll = 0
- """
- The following are private helper methods to read and write registers, as well as to convert the read values to the correct unit.
- """
-
def _int16(self, d):
return d if d < 0x8000 else d - 0x10000
@@ -223,6 +227,12 @@ def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
:return: False if timeout occurred, otherwise True
:rtype: bool
"""
+ # Stop timer
+ self._stop_timer()
+
+ # Reset member variables
+ self._reset_member_variables()
+
# Set BOOT and SW_RESET bits
self.reg_ctrl3_c_byte[0] = self._getreg(LSM_REG_CTRL3_C)
self.reg_ctrl3_c_bits.BOOT = 1
From c742e2ff3cd2b692fa899b3dac4a01acf4046cd6 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Tue, 18 Jul 2023 11:17:13 -0600
Subject: [PATCH 21/22] Make gyro rate functions public
---
XRPLib/imu.py | 132 +++++++++++++++++++++++++-------------------------
1 file changed, 66 insertions(+), 66 deletions(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index 149d6fc..a8bbeb8 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -137,70 +137,6 @@ def _raw_to_mg(self, raw):
def _raw_to_mdps(self, raw):
return self._int16((raw[1] << 8) | raw[0]) * LSM_MDPS_PER_LSB_125DPS * self._gyro_scale_factor
-
- def _get_gyro_x_rate(self):
- """
- Individual axis read for the Gyroscope's X-axis, in mg
- """
- # Burst read data registers
- raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 2)
-
- # Convert raw data to mdps
- return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
-
- def _get_gyro_y_rate(self):
- """
- Individual axis read for the Gyroscope's Y-axis, in mg
- """
- # Burst read data registers
- raw_bytes = self._getregs(LSM_REG_OUTY_L_G, 2)
-
- # Convert raw data to mdps
- return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[1]
-
- def _get_gyro_z_rate(self):
- """
- Individual axis read for the Gyroscope's Z-axis, in mg
- """
- # Burst read data registers
- raw_bytes = self._getregs(LSM_REG_OUTZ_L_G, 2)
-
- # Convert raw data to mdps
- return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[2]
-
- def _get_gyro_rates(self):
- """
- Retrieves the array of readings from the Gyroscope, in mdps
- The order of the values is x, y, z.
- """
- # Burst read data registers
- raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 6)
-
- # Convert raw data to mdps
- self.irq_v[1][0] = self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
- self.irq_v[1][1] = self._raw_to_mdps(raw_bytes[2:4]) - self.gyro_offsets[1]
- self.irq_v[1][2] = self._raw_to_mdps(raw_bytes[4:6]) - self.gyro_offsets[2]
-
- return self.irq_v[1]
-
- def _get_acc_gyro_rates(self):
- """
- Get the accelerometer and gyroscope values in mg and mdps in the form of a 2D array.
- The first row is the acceleration values, the second row is the gyro values.
- The order of the values is x, y, z.
- """
- # Burst read data registers
- raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 12)
-
- # Convert raw data to mg's and mdps
- self.irq_v[0][0] = self._raw_to_mg(raw_bytes[6:8]) - self.acc_offsets[0]
- self.irq_v[0][1] = self._raw_to_mg(raw_bytes[8:10]) - self.acc_offsets[1]
- self.irq_v[0][2] = self._raw_to_mg(raw_bytes[10:12]) - self.acc_offsets[2]
- self.irq_v[1][0] = self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
- self.irq_v[1][1] = self._raw_to_mdps(raw_bytes[2:4]) - self.gyro_offsets[1]
- self.irq_v[1][2] = self._raw_to_mdps(raw_bytes[4:6]) - self.gyro_offsets[2]
-
- return self.irq_v
"""
Public facing API Methods
@@ -300,6 +236,70 @@ def get_acc_rates(self):
self.irq_v[0][2] = self._raw_to_mg(raw_bytes[4:6]) - self.acc_offsets[2]
return self.irq_v[0]
+
+ def get_gyro_x_rate(self):
+ """
+ Individual axis read for the Gyroscope's X-axis, in mg
+ """
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 2)
+
+ # Convert raw data to mdps
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
+
+ def get_gyro_y_rate(self):
+ """
+ Individual axis read for the Gyroscope's Y-axis, in mg
+ """
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTY_L_G, 2)
+
+ # Convert raw data to mdps
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[1]
+
+ def get_gyro_z_rate(self):
+ """
+ Individual axis read for the Gyroscope's Z-axis, in mg
+ """
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTZ_L_G, 2)
+
+ # Convert raw data to mdps
+ return self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[2]
+
+ def get_gyro_rates(self):
+ """
+ Retrieves the array of readings from the Gyroscope, in mdps
+ The order of the values is x, y, z.
+ """
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 6)
+
+ # Convert raw data to mdps
+ self.irq_v[1][0] = self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
+ self.irq_v[1][1] = self._raw_to_mdps(raw_bytes[2:4]) - self.gyro_offsets[1]
+ self.irq_v[1][2] = self._raw_to_mdps(raw_bytes[4:6]) - self.gyro_offsets[2]
+
+ return self.irq_v[1]
+
+ def get_acc_gyro_rates(self):
+ """
+ Get the accelerometer and gyroscope values in mg and mdps in the form of a 2D array.
+ The first row is the acceleration values, the second row is the gyro values.
+ The order of the values is x, y, z.
+ """
+ # Burst read data registers
+ raw_bytes = self._getregs(LSM_REG_OUTX_L_G, 12)
+
+ # Convert raw data to mg's and mdps
+ self.irq_v[0][0] = self._raw_to_mg(raw_bytes[6:8]) - self.acc_offsets[0]
+ self.irq_v[0][1] = self._raw_to_mg(raw_bytes[8:10]) - self.acc_offsets[1]
+ self.irq_v[0][2] = self._raw_to_mg(raw_bytes[10:12]) - self.acc_offsets[2]
+ self.irq_v[1][0] = self._raw_to_mdps(raw_bytes[0:2]) - self.gyro_offsets[0]
+ self.irq_v[1][1] = self._raw_to_mdps(raw_bytes[2:4]) - self.gyro_offsets[1]
+ self.irq_v[1][2] = self._raw_to_mdps(raw_bytes[4:6]) - self.gyro_offsets[2]
+
+ return self.irq_v
def get_pitch(self):
"""
@@ -503,7 +503,7 @@ def calibrate(self, calibration_time:float=1, vertical_axis:int= 2):
time.sleep(.1)
start_time = time.ticks_ms()
while time.ticks_ms() < start_time + calibration_time*1000:
- cur_vals = self._get_acc_gyro_rates()
+ cur_vals = self.get_acc_gyro_rates()
# Accelerometer averages
avg_vals[0][0] += cur_vals[0][0]
avg_vals[0][1] += cur_vals[0][1]
@@ -538,7 +538,7 @@ def _stop_timer(self):
def _update_imu_readings(self):
# Called every tick through a callback timer
- self._get_gyro_rates()
+ self.get_gyro_rates()
delta_pitch = self.irq_v[1][0] / 1000 / self.timer_frequency
delta_roll = self.irq_v[1][1] / 1000 / self.timer_frequency
delta_yaw = self.irq_v[1][2] / 1000 / self.timer_frequency
From 37d5b9b3b1515235e9680642bc9ec92716d8c435 Mon Sep 17 00:00:00 2001
From: Dryw Wade <dryw.wade@sparkfun.com>
Date: Tue, 18 Jul 2023 11:18:41 -0600
Subject: [PATCH 22/22] Add WHO_AM_I value to constants in imu_defs.py
---
XRPLib/imu.py | 2 +-
XRPLib/imu_defs.py | 1 +
2 files changed, 2 insertions(+), 1 deletion(-)
diff --git a/XRPLib/imu.py b/XRPLib/imu.py
index a8bbeb8..c4c7fbe 100644
--- a/XRPLib/imu.py
+++ b/XRPLib/imu.py
@@ -150,7 +150,7 @@ def is_connected(self):
:rtype: bool
"""
who_am_i = self._getreg(LSM_REG_WHO_AM_I)
- return who_am_i == 0x6C
+ return who_am_i == LSM_WHO_AM_I_VALUE
def reset(self, wait_for_reset = True, wait_timeout_ms = 100):
"""
diff --git a/XRPLib/imu_defs.py b/XRPLib/imu_defs.py
index 7ccdcb3..c8732de 100644
--- a/XRPLib/imu_defs.py
+++ b/XRPLib/imu_defs.py
@@ -78,5 +78,6 @@
"""
Other contants
"""
+LSM_WHO_AM_I_VALUE = 0x6C
LSM_MG_PER_LSB_2G = 0.061
LSM_MDPS_PER_LSB_125DPS = 4.375
\ No newline at end of file