From 133517fea7a3bee29082a252a285c12120c58f76 Mon Sep 17 00:00:00 2001
From: Eiren Rain <eirenliel@users.noreply.github.com>
Date: Sat, 6 Mar 2021 04:27:22 +0300
Subject: [PATCH] Minor cleanup for different sensors

---
 src/configuration.h |  1 +
 src/defines.h       | 23 +++++++++++++++++++++++
 src/main.cpp        |  8 +++++++-
 src/sensor.h        | 24 ++++++++++++------------
 4 files changed, 43 insertions(+), 13 deletions(-)

diff --git a/src/configuration.h b/src/configuration.h
index 874ff1f..30f8665 100644
--- a/src/configuration.h
+++ b/src/configuration.h
@@ -18,6 +18,7 @@ struct CalibrationConfig {
 struct DeviceConfig {
     CalibrationConfig calibration;
     int deviceId;
+    int deviceMode;
 };
 
 void initializeConfig();
diff --git a/src/defines.h b/src/defines.h
index 208e687..98b9254 100644
--- a/src/defines.h
+++ b/src/defines.h
@@ -14,6 +14,29 @@
   SDA/SCL to Pin4 and Pin5 of Adafruit Huzzah, respectively
 */
 
+#define IMU_MPU9250 1
+#define IMU_MPU6500 2
+#define IMU_BNO080 3
+
+#define IMU IMU_BNO080
+
+#if IMU == IMU_BNO080
+  #define IMU_NAME "BNO080"
+  #define IMU_HAS_ACCELL true
+  #define IMU_HAS_GYRO true
+  #define IMU_HAS_MAG true
+#elif IMU == IMU_MPU9250
+  #define IMU_NAME "MPU9250"
+  #define IMU_HAS_ACCELL true
+  #define IMU_HAS_GYRO true
+  #define IMU_HAS_MAG true
+#elif IMU == IMU_MPU6500
+  #define IMU_NAME "MPU6500"
+  #define IMU_HAS_ACCELL true
+  #define IMU_HAS_GYRO true
+  #define IMU_HAS_MAG false
+#endif
+
 ///////////////////////////////////////////////////////////////////
 //Debug information
 ///////////////////////////////////////////////////////////////////
diff --git a/src/main.cpp b/src/main.cpp
index 20c933a..1217ea7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -26,7 +26,13 @@
 #include "defines.h"
 #include "credentials.h"
 
-BNO080Sensor sensor{};
+#if IMU == IMU_BNO080
+    BNO080Sensor sensor{};
+#elif IMU == IMU_MPU9250
+    MPU9250Sensor sensor{};
+#elif IMU == IMU_MPU6500
+    MPU6050Sensor sensor{};
+#endif
 DeviceConfig config{};
 
 bool isCalibrating = false;
diff --git a/src/sensor.h b/src/sensor.h
index 48c46d9..e394708 100644
--- a/src/sensor.h
+++ b/src/sensor.h
@@ -24,10 +24,10 @@ class BNO080Sensor : public Sensor {
     public:
         BNO080Sensor() = default;
         ~BNO080Sensor() override = default;
-        void motionSetup(DeviceConfig * config) final;
-        void motionLoop() final;
-        void sendData() final;
-        void startCalibration(int calibrationType) final;
+        void motionSetup(DeviceConfig * config) override final;
+        void motionLoop() override final;
+        void sendData() override final;
+        void startCalibration(int calibrationType) override final;
     private:
         BNO080 imu {};
         bool newData {false};
@@ -46,10 +46,10 @@ class MPU6050Sensor : public MPUSensor {
     public:
         MPU6050Sensor() = default;
         ~MPU6050Sensor() override  = default;
-        void motionSetup(DeviceConfig * config) final;
-        void motionLoop() final;
-        void sendData() final;
-        void startCalibration(int calibrationType) final;
+        void motionSetup(DeviceConfig * config) override final;
+        void motionLoop() override final;
+        void sendData() override final;
+        void startCalibration(int calibrationType) override final;
     private:
         Quaternion rawQuat {};
         // MPU dmp control/status vars
@@ -65,10 +65,10 @@ class MPU9250Sensor : public MPUSensor {
     public:
         MPU9250Sensor() = default;
         ~MPU9250Sensor() override  = default;
-        void motionSetup(DeviceConfig * config) final;
-        void motionLoop() final;
-        void sendData() final;
-        void startCalibration(int calibrationType) final;
+        void motionSetup(DeviceConfig * config) override final;
+        void motionLoop() override final;
+        void sendData() override final;
+        void startCalibration(int calibrationType) override final;
         void getMPUScaled();
         void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
     private: