From 4682eaf581df14696cffbdf8b987d0ae480c1532 Mon Sep 17 00:00:00 2001
From: apirrone <awanegenebayo@gmail.com>
Date: Mon, 17 Mar 2025 09:58:39 +0100
Subject: [PATCH] upside down imu option

---
 mini_bdx_runtime/mini_bdx_runtime/imu.py     | 15 ++++++-------
 mini_bdx_runtime/mini_bdx_runtime/raw_imu.py | 22 +++++++++++---------
 2 files changed, 20 insertions(+), 17 deletions(-)

diff --git a/mini_bdx_runtime/mini_bdx_runtime/imu.py b/mini_bdx_runtime/mini_bdx_runtime/imu.py
index 9bbe7eb..0898dbc 100644
--- a/mini_bdx_runtime/mini_bdx_runtime/imu.py
+++ b/mini_bdx_runtime/mini_bdx_runtime/imu.py
@@ -15,7 +15,9 @@ from scipy.spatial.transform import Rotation as R
 
 # TODO filter spikes
 class Imu:
-    def __init__(self, sampling_freq, user_pitch_bias=0, calibrate=False):
+    def __init__(
+        self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True
+    ):
         self.sampling_freq = sampling_freq
         self.user_pitch_bias = user_pitch_bias
         self.nominal_pitch_bias = 25
@@ -33,13 +35,14 @@ class Imu:
         # self.imu.mode = adafruit_bno055.NDOF_MODE
         # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE
 
-        self.imu.axis_remap = (
+        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_NEGATIVE,
             )
 
         self.pitch_bias = self.nominal_pitch_bias + self.user_pitch_bias
@@ -79,7 +82,6 @@ class Imu:
             print("imu_calib_data.pkl not found")
             print("Imu is running uncalibrated")
 
-
         self.last_imu_data = [0, 0, 0, 0]
         self.imu_queue = Queue(maxsize=1)
         Thread(target=self.imu_worker, daemon=True).start()
@@ -134,10 +136,9 @@ class Imu:
             return None
 
 
-
 if __name__ == "__main__":
-    # imu = Imu(50, calibrate=True)
-    imu = Imu(50)
+    imu = Imu(50, calibrate=True, upside_down=False)
+    # imu = Imu(50, upside_down=False)
     while True:
         data = imu.get_data()
         # print(data)
diff --git a/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py b/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
index 6dcaabc..a67ebd6 100644
--- a/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
+++ b/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py
@@ -12,7 +12,7 @@ import time
 
 # TODO filter spikes
 class Imu:
-    def __init__(self, sampling_freq, user_pitch_bias=0, calibrate=False):
+    def __init__(self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True):
         self.sampling_freq = sampling_freq
         self.calibrate = calibrate
 
@@ -25,14 +25,16 @@ class Imu:
         # self.imu.mode = adafruit_bno055.NDOF_MODE
         # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE
 
-        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
-            )
+
+        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
+                )
 
         if self.calibrate:
             self.imu.mode = adafruit_bno055.NDOF_MODE
@@ -140,7 +142,7 @@ class Imu:
 
 
 if __name__ == "__main__":
-    imu = Imu(50)
+    imu = Imu(50, upside_down=False)
     while True:
         data = imu.get_data()
         # print(data)