Fix various issues (#144)

* Add debug flag for configuration

* Fix debug logging in ICM-20948

* Fix build for trace builds

* Add `Sensor#postSetup()`

This is done to prevent IMU timeouts when WiFi takes longer than a second to connect.

* Filter out FIFOMoreDataAvailable in logging for ICM-20948

src/configuration/Configuration.cpp

@@ -83,7 +83,7 @@ namespace SlimeVR {
 
             m_Logger.info("Loaded configuration");
 
-#ifdef FULL_DEBUG
+#ifdef DEBUG_CONFIGURATION
             print();
 #endif
         }

src/debug.h

@@ -38,8 +38,9 @@
 #define LOG_LEVEL LOG_LEVEL_DEBUG
 
 #if LOG_LEVEL == LOG_LEVEL_TRACE
-#define DEBUG_SENSOR
-#define DEBUG_NETWORK
+    #define DEBUG_SENSOR
+    #define DEBUG_NETWORK
+    #define DEBUG_CONFIGURATION
 #endif
 
 #define serialDebug false // Set to true to get Serial output for debugging

src/main.cpp

@@ -88,6 +88,8 @@ void setup()
 
     statusManager.setStatus(SlimeVR::Status::LOADING, false);
 
+    sensorManager.postSetup();
+
     loopTime = micros();
 }
 

src/sensors/SensorManager.cpp

@@ -120,6 +120,12 @@ namespace SlimeVR
             }
         }
 
+        void SensorManager::postSetup()
+        {
+            m_Sensor1->postSetup();
+            m_Sensor2->postSetup();
+        }
+
         void SensorManager::update()
         {
             // Gather IMU data

src/sensors/SensorManager.h

@@ -45,6 +45,8 @@ namespace SlimeVR
             }
 
             void setup();
+            void postSetup();
+
             void update();
 
             Sensor *getFirst() { return m_Sensor1; };

src/sensors/bmi160sensor.cpp

@@ -231,7 +231,7 @@ void BMI160Sensor::startCalibration(int calibrationType) {
     m_Calibration.G_off[2] = rawGxyz[2] / gyroCalibrationSamples;
 
 #ifdef DEBUG_SENSOR
-    m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Calibration[sensorId].G_off));
+    m_Logger.trace("Gyro calibration results: %f %f %f", UNPACK_VECTOR_ARRAY(m_Calibration.G_off));
 #endif
 
     // Blink calibrating led before user should rotate the sensor

src/sensors/bno080sensor.h

@@ -34,6 +34,10 @@ public:
         : Sensor("BNO080Sensor", type, id, address, rotation), m_IntPin(intPin) {};
     ~BNO080Sensor(){};
     void motionSetup() override final;
+    void postSetup() override {
+        lastData = millis();
+    }
+
     void motionLoop() override final;
     void sendData() override final;
     void startCalibration(int calibrationType) override final;

src/sensors/icm20948sensor.cpp

@@ -136,7 +136,7 @@ void ICM20948Sensor::load_bias() {
     imu.GetBiasCPassY(&bias_compass[1]);
     imu.GetBiasCPassZ(&bias_compass[2]);
 
-        #ifdef FULL_DEBUG
+        #ifdef DEBUG_SENSOR
     m_Logger.trace("All set bias should be 90");
 
     m_Logger.trace("Gyrometer bias    : [%d, %d, %d]", UNPACK_VECTOR_ARRAY(bias_gyro));
@@ -148,7 +148,7 @@ void ICM20948Sensor::load_bias() {
 }
 
 void ICM20948Sensor::motionSetup() {
-    #ifdef FULL_DEBUG
+    #ifdef DEBUG_SENSOR
         imu.enableDebugging(Serial);
     #endif
     // SparkFun_ICM-20948_ArduinoLibrary only supports 0x68 or 0x69 via boolean, if something else throw a error
@@ -399,13 +399,14 @@ void ICM20948Sensor::motionLoop() {
         }
         else 
         {
-            if (readStatus == ICM_20948_Stat_FIFONoDataAvail || lastData + 1000 < millis()) 
-            {
+            if (readStatus == ICM_20948_Stat_FIFONoDataAvail || lastData + 1000 < millis()) {
                 dataavaliable = false;
+            } else if (readStatus == ICM_20948_Stat_FIFOMoreDataAvail) {
+                dataavaliable = true;
             }
+            // Sorry for this horrible formatting
 #ifdef DEBUG_SENSOR
-            else 
-            {
+            else {
                 m_Logger.trace("e0x%02x", readStatus);
             }
 #endif

src/sensors/icm20948sensor.h

@@ -33,7 +33,12 @@ public:
     ICM20948Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM20948Sensor", IMU_ICM20948, id, address, rotation) {}
     ~ICM20948Sensor() override = default;
     void motionSetup() override final;
+    void postSetup() override {
+        this->lastData = millis();
+    }
+
     void motionLoop() override final;
+
     void sendData() override final;
     void startCalibration(int calibrationType) override final;
     void save_bias(bool repeat);

src/sensors/sensor.h

@@ -48,6 +48,7 @@ public:
 
     virtual ~Sensor(){};
     virtual void motionSetup(){};
+    virtual void postSetup(){};
     virtual void motionLoop(){};
     virtual void sendData();
     virtual void startCalibration(int calibrationType){};