Multiple debugging updates to serial interface

src/debug.h

@@ -48,6 +48,7 @@
 #define serialDebug false // Set to true to get Serial output for debugging
 #define serialBaudRate 115200
 #define LED_INTERVAL_STANDBY 10000
+#define PRINT_STATE_EVERY_MS 60000
 
 // Determines how often we sample and send data
 #define samplingRateInMillis 10

src/main.cpp

@@ -45,6 +45,7 @@ int sensorToCalibrate = -1;
 bool blinking = false;
 unsigned long blinkStart = 0;
 unsigned long loopTime = 0;
+unsigned long lastStatePrint = 0;
 bool secondImuActive = false;
 BatteryMonitor battery;
 
@@ -113,7 +114,6 @@ void loop()
     sensorManager.update();
     battery.Loop();
     ledManager.update();
-
 #ifdef TARGET_LOOPTIME_MICROS
     long elapsed = (micros() - loopTime);
     if (elapsed < TARGET_LOOPTIME_MICROS)
@@ -132,4 +132,11 @@ void loop()
     }
     loopTime = micros();
 #endif
+    #if defined(PRINT_STATE_EVERY_MS) && PRINT_STATE_EVERY_MS > 0
+        unsigned long now = millis();
+        if(lastStatePrint + PRINT_STATE_EVERY_MS < now) {
+            lastStatePrint = now;
+            SerialCommands::printState();
+        }
+    #endif
 }

src/network/wifihandler.cpp

@@ -127,6 +127,10 @@ void onConnected() {
     wifiHandlerLogger.info("Connected successfully to SSID '%s', ip address %s", WiFi.SSID().c_str(), WiFi.localIP().toString().c_str());
 }
 
+uint8_t WiFiNetwork::getWiFiState() {
+    return wifiState;
+}
+
 void WiFiNetwork::upkeep() {
     upkeepProvisioning();
     if(WiFi.status() != WL_CONNECTED) {

src/network/wifihandler.h

@@ -35,6 +35,7 @@ namespace WiFiNetwork {
     void upkeep();
     void setWiFiCredentials(const char * SSID, const char * pass);
     IPAddress getAddress();
+    uint8_t getWiFiState();
 }
 
 /** Wifi Reconnection Statuses **/

src/sensors/bno080sensor.cpp

@@ -72,8 +72,6 @@ void BNO080Sensor::motionSetup()
     #endif
 #endif
 
-    imu.enableTapDetector(100);
-
 #if ENABLE_INSPECTION
     imu.enableRawGyro(10);
     imu.enableRawAccelerometer(10);
@@ -221,9 +219,7 @@ void BNO080Sensor::sendData()
         Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId);
 
 #if SEND_ACCELERATION
-        {
-            Network::sendAccel(this->acceleration, this->sensorId);
-        }
+        Network::sendAccel(this->acceleration, this->sensorId);
 #endif
 
 #if !USE_6_AXIS

src/sensors/mpu6050sensor.cpp

@@ -45,7 +45,7 @@ void MPU6050Sensor::motionSetup()
     imu.initialize(addr);
     if (!imu.testConnection())
     {
-        m_Logger.fatal("Can't connect to %s (reported device ID 0x%02x) at address 0x%02x",getIMUNameByType(sensorType), imu.getDeviceID(), addr);
+        m_Logger.fatal("Can't connect to %s (reported device ID 0x%02x) at address 0x%02x", getIMUNameByType(sensorType), imu.getDeviceID(), addr);
         return;
     }
 

src/sensors/sensor.h

@@ -64,6 +64,9 @@ class Sensor
     uint8_t getSensorType() {
         return sensorType;
     };
+    Quat& getQuaternion() {
+        return quaternion;
+    };
 
     bool hadData = false;
 protected:

src/serial/serialcommands.cpp

@@ -27,6 +27,7 @@
 #include <CmdCallback.hpp>
 #include "GlobalVars.h"
 #include "batterymonitor.h"
+#include "utils.h"
 
 #if ESP32
     #include "nvs_flash.h"
@@ -53,30 +54,43 @@ namespace SerialCommands {
         }
     }
 
+    void printState() {
+        logger.info(
+            "SlimeVR Tracker, board: %d, hardware: %d, build: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
+            BOARD,
+            HARDWARE_MCU,
+            FIRMWARE_BUILD_NUMBER,
+            FIRMWARE_VERSION,
+            WiFiNetwork::getAddress().toString().c_str(),
+            WiFi.macAddress().c_str(),
+            statusManager.getStatus(),
+            WiFiNetwork::getWiFiState()
+        );
+        Sensor* sensor1 = sensorManager.getFirst();
+        Sensor* sensor2 = sensorManager.getSecond();
+        logger.info(
+            "Sensor 1: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
+            getIMUNameByType(sensor1->getSensorType()),
+            UNPACK_QUATERNION(sensor1->getQuaternion()),
+            sensor1->isWorking() ? "true" : "false",
+            sensor1->hadData ? "true" : "false"
+        );
+        logger.info(
+            "Sensor 2: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
+            getIMUNameByType(sensor2->getSensorType()),
+            UNPACK_QUATERNION(sensor2->getQuaternion()),
+            sensor2->isWorking() ? "true" : "false",
+            sensor2->hadData ? "true" : "false"
+        );
+    }
+
     void cmdGet(CmdParser * parser) {
         if (parser->getParamCount() < 2) {
             return;
         }
 
         if (parser->equalCmdParam(1, "INFO")) {
-            logger.info(
-                "SlimeVR Tracker, board: %d, hardware: %d, build: %d, firmware: %s, address: %s",
-                BOARD,
-                HARDWARE_MCU,
-                FIRMWARE_BUILD_NUMBER,
-                FIRMWARE_VERSION,
-                WiFiNetwork::getAddress().toString().c_str()
-            );
-            Sensor* sensor1 = sensorManager.getFirst();
-            Sensor* sensor2 = sensorManager.getSecond();
-            logger.info(
-                "Sensor 1: %s",
-                getIMUNameByType(sensor1->getSensorType())
-            );
-            logger.info(
-                "Sensor 2: %s",
-                getIMUNameByType(sensor2->getSensorType())
-            );
+            printState();
         }
 
         if (parser->equalCmdParam(1, "CONFIG")) {
@@ -121,18 +135,24 @@ namespace SerialCommands {
 
         if (parser->equalCmdParam(1, "TEST")) {
             logger.info(
-                "[TEST] Board: %d, hardware: %d, build: %d, firmware: %s, mac: %s",
+                "[TEST] Board: %d, hardware: %d, build: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
                 BOARD,
                 HARDWARE_MCU,
                 FIRMWARE_BUILD_NUMBER,
                 FIRMWARE_VERSION,
-                WiFi.macAddress()
+                WiFiNetwork::getAddress().toString().c_str(),
+                WiFi.macAddress().c_str(),
+                statusManager.getStatus(),
+                WiFiNetwork::getWiFiState()
             );
             Sensor* sensor1 = sensorManager.getFirst();
             sensor1->motionLoop();
             logger.info(
-                "[TEST] Sensor 1: %s",
-                getIMUNameByType(sensor1->getSensorType())
+                "[TEST] Sensor 1: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
+                getIMUNameByType(sensor1->getSensorType()),
+                UNPACK_QUATERNION(sensor1->getQuaternion()),
+                sensor1->isWorking() ? "true" : "false",
+                sensor1->hadData ? "true" : "false"
             );
             if(!sensor1->hadData) {
                 logger.error("[TEST] Sensor 1 didn't send any data yet!");
@@ -142,10 +162,6 @@ namespace SerialCommands {
         }
     }
 
-    void cmdReport(CmdParser * parser) {
-        // TODO Health and status report
-    }
-
     void cmdReboot(CmdParser * parser) {
         logger.info("REBOOT");
         ESP.restart();
@@ -160,7 +176,7 @@ namespace SerialCommands {
         #if ESP8266
             ESP.eraseConfig(); // Clear ESP config
         #elif ESP32
-                nvs_flash_erase();
+            nvs_flash_erase();
         #else
             #warning SERIAL COMMAND FACTORY RESET NOT SUPPORTED
             logger.info("FACTORY RESET NOT SUPPORTED");
@@ -180,7 +196,6 @@ namespace SerialCommands {
         cmdCallbacks.addCmd("SET", &cmdSet);
         cmdCallbacks.addCmd("GET", &cmdGet);
         cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
-        cmdCallbacks.addCmd("REP", &cmdReport);
         cmdCallbacks.addCmd("REBOOT", &cmdReboot);
 
     }

src/serial/serialcommands.h

@@ -27,6 +27,7 @@
 namespace SerialCommands {
     void setUp();
     void update();
+    void printState();
 }
 
 #endif // SLIMEVR_SERIALCOMMANDS_H_

src/status/StatusManager.h

@@ -13,6 +13,9 @@ namespace SlimeVR
         public:
             void setStatus(Status status, bool value);
             bool hasStatus(Status status);
+            uint32_t getStatus() {
+                return m_Status;
+            };
 
         private:
             uint32_t m_Status;