From 7309173bc5eaf4f988cc179b3eb46fc727d9748c Mon Sep 17 00:00:00 2001
From: Eiren Rain <eirenliel@users.noreply.github.com>
Date: Sun, 14 Mar 2021 15:17:48 +0300
Subject: [PATCH] Better support for different Bosch sensors, easy swap between
 055/080/085 with one define, added compilation error just in case

---
 src/bno080sensor.cpp |  6 +++++-
 src/defines.h        | 21 +++++++++++++++++++--
 src/main.cpp         |  4 +++-
 3 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/src/bno080sensor.cpp b/src/bno080sensor.cpp
index 6159a6e..1a5de3f 100644
--- a/src/bno080sensor.cpp
+++ b/src/bno080sensor.cpp
@@ -24,6 +24,7 @@
 #include "BNO080.h"
 #include "sensor.h"
 #include "udpclient.h"
+#include "defines.h"
 
 void BNO080Sensor::motionSetup(DeviceConfig * config)
 {
@@ -39,7 +40,10 @@ void BNO080Sensor::motionSetup(DeviceConfig * config)
     }
     Serial.println("Connected to BNO08X");
     Wire.setClock(400000);
-    imu.enableARVRStabilizedGameRotationVector(10);
+    if(BNO_HASARVR_STABILIZATION)
+        imu.enableARVRStabilizedGameRotationVector(10);
+    else
+        imu.enableGameRotationVector(10);
 }
 
 void BNO080Sensor::motionLoop()
diff --git a/src/defines.h b/src/defines.h
index 74792a3..547ca4f 100644
--- a/src/defines.h
+++ b/src/defines.h
@@ -24,14 +24,29 @@
 #define IMU_MPU9250 1
 #define IMU_MPU6500 2
 #define IMU_BNO080 3
+#define IMU_BNO085 4
+#define IMU_BNO055 5
 
-#define IMU IMU_BNO080
+#define IMU IMU_BNO085
 
-#if IMU == IMU_BNO080
+#if IMU == IMU_BNO085
+  #define IMU_NAME "BNO085"
+  #define IMU_HAS_ACCELL true
+  #define IMU_HAS_GYRO true
+  #define IMU_HAS_MAG true
+  #define BNO_HASARVR_STABILIZATION true
+#elif IMU == IMU_BNO080
   #define IMU_NAME "BNO080"
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true
   #define IMU_HAS_MAG true
+  #define BNO_HASARVR_STABILIZATION false
+#elif IMU == IMU_BNO055
+  #define IMU_NAME "BNO055"
+  #define IMU_HAS_ACCELL true
+  #define IMU_HAS_GYRO true
+  #define IMU_HAS_MAG true
+  #define BNO_HASARVR_STABILIZATION false
 #elif IMU == IMU_MPU9250
   #define IMU_NAME "MPU9250"
   #define IMU_HAS_ACCELL true
@@ -42,6 +57,8 @@
   #define IMU_HAS_ACCELL true
   #define IMU_HAS_GYRO true
   #define IMU_HAS_MAG false
+#else
+    #error Select IMU in defines.h
 #endif
 
 //Debug information
diff --git a/src/main.cpp b/src/main.cpp
index 63bea27..adaab72 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -29,12 +29,14 @@
 #include "defines.h"
 #include "credentials.h"
 
-#if IMU == IMU_BNO080
+#if IMU == IMU_BNO080 || IMU == IMU_BNO085 || IMU == IMU_BNO055
     BNO080Sensor sensor{};
 #elif IMU == IMU_MPU9250
     MPU9250Sensor sensor{};
 #elif IMU == IMU_MPU6500
     MPU6050Sensor sensor{};
+#else
+    #error Unsupported IMU
 #endif
 DeviceConfig config{};