From f8751e2bbe84705a32c26868bffecaa2066e3cff Mon Sep 17 00:00:00 2001
From: apirrone <awanegenebayo@gmail.com>
Date: Mon, 17 Mar 2025 10:51:16 +0100
Subject: [PATCH] upside down imu option

---
 mini_bdx_runtime/mini_bdx_runtime/imu.py     | 13 +++++++
 mini_bdx_runtime/mini_bdx_runtime/raw_imu.py | 37 ++++++++++++++------
 setup.cfg                                    |  2 +-
 3 files changed, 40 insertions(+), 12 deletions(-)

diff --git a/mini_bdx_runtime/mini_bdx_runtime/imu.py b/mini_bdx_runtime/mini_bdx_runtime/imu.py
index 9a35a34..bcdcb55 100644
--- a/mini_bdx_runtime/mini_bdx_runtime/imu.py
+++ b/mini_bdx_runtime/mini_bdx_runtime/imu.py
@@ -44,6 +44,15 @@ class Imu:
                 adafruit_bno055.AXIS_REMAP_NEGATIVE,
                 adafruit_bno055.AXIS_REMAP_NEGATIVE,
             )
+        else:
+            self.imu.axis_remap = (
+                adafruit_bno055.AXIS_REMAP_Y,
+                adafruit_bno055.AXIS_REMAP_X,
+                adafruit_bno055.AXIS_REMAP_Z,
+                adafruit_bno055.AXIS_REMAP_NEGATIVE,
+                adafruit_bno055.AXIS_REMAP_POSITIVE,
+                adafruit_bno055.AXIS_REMAP_POSITIVE,
+            )
 
         self.pitch_bias = self.nominal_pitch_bias + self.user_pitch_bias
 
@@ -75,9 +84,13 @@ class Imu:
 
         if os.path.exists("imu_calib_data.pkl"):
             imu_calib_data = pickle.load(open("imu_calib_data.pkl", "rb"))
+            self.imu.mode = adafruit_bno055.CONFIG_MODE
+            time.sleep(0.1)
             self.imu.offsets_accelerometer = imu_calib_data["offsets_accelerometer"]
             self.imu.offsets_gyroscope = imu_calib_data["offsets_gyroscope"]
             self.imu.offsets_magnetometer = imu_calib_data["offsets_magnetometer"]
+            self.imu.mode = adafruit_bno055.IMUPLUS_MODE
+            time.sleep(0.1)
         else:
             print("imu_calib_data.pkl not found")
             print("Imu is running uncalibrated")
diff --git a/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py b/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
index a67ebd6..ce24627 100644
--- a/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
+++ b/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
@@ -12,7 +12,9 @@ import time
 
 # TODO filter spikes
 class Imu:
-    def __init__(self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True):
+    def __init__(
+        self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True
+    ):
         self.sampling_freq = sampling_freq
         self.calibrate = calibrate
 
@@ -20,21 +22,30 @@ class Imu:
         self.imu = adafruit_bno055.BNO055_I2C(i2c)
 
         # self.imu.mode = adafruit_bno055.IMUPLUS_MODE
-        self.imu.mode = adafruit_bno055.ACCGYRO_MODE
+        # self.imu.mode = adafruit_bno055.ACCGYRO_MODE
         # self.imu.mode = adafruit_bno055.GYRONLY_MODE
-        # self.imu.mode = adafruit_bno055.NDOF_MODE
+        self.imu.mode = adafruit_bno055.NDOF_MODE
         # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE
 
-
         if upside_down:
             self.imu.axis_remap = (
-                    adafruit_bno055.AXIS_REMAP_Y,
-                    adafruit_bno055.AXIS_REMAP_X,
-                    adafruit_bno055.AXIS_REMAP_Z,
-                    adafruit_bno055.AXIS_REMAP_NEGATIVE,
-                    adafruit_bno055.AXIS_REMAP_NEGATIVE,
-                    adafruit_bno055.AXIS_REMAP_NEGATIVE
-                )
+                adafruit_bno055.AXIS_REMAP_Y,
+                adafruit_bno055.AXIS_REMAP_X,
+                adafruit_bno055.AXIS_REMAP_Z,
+                adafruit_bno055.AXIS_REMAP_NEGATIVE,
+                adafruit_bno055.AXIS_REMAP_NEGATIVE,
+                adafruit_bno055.AXIS_REMAP_NEGATIVE,
+            )
+
+        else:
+            self.imu.axis_remap = (
+                adafruit_bno055.AXIS_REMAP_Y,
+                adafruit_bno055.AXIS_REMAP_X,
+                adafruit_bno055.AXIS_REMAP_Z,
+                adafruit_bno055.AXIS_REMAP_NEGATIVE,
+                adafruit_bno055.AXIS_REMAP_POSITIVE,
+                adafruit_bno055.AXIS_REMAP_POSITIVE,
+            )
 
         if self.calibrate:
             self.imu.mode = adafruit_bno055.NDOF_MODE
@@ -64,9 +75,13 @@ class Imu:
 
         if os.path.exists("imu_calib_data.pkl"):
             imu_calib_data = pickle.load(open("imu_calib_data.pkl", "rb"))
+            self.imu.mode = adafruit_bno055.CONFIG_MODE
+            time.sleep(0.1)
             self.imu.offsets_accelerometer = imu_calib_data["offsets_accelerometer"]
             self.imu.offsets_gyroscope = imu_calib_data["offsets_gyroscope"]
             self.imu.offsets_magnetometer = imu_calib_data["offsets_magnetometer"]
+            self.imu.mode = adafruit_bno055.NDOF_MODE
+            time.sleep(0.1)
         else:
             print("imu_calib_data.pkl not found")
             print("Imu is running uncalibrated")
diff --git a/setup.cfg b/setup.cfg
index ec0c028..8bd7cc1 100644
--- a/setup.cfg
+++ b/setup.cfg
@@ -23,7 +23,7 @@ install_requires =
     adafruit-circuitpython-bno055==5.4.13
     scipy==1.15.1
     pygame==2.6.0
-    adafruit_exended_bus
+    adafruit_extended_bus
 [options.packages.find]
 where=mini_bdx_runtime