SlimeVR BNO Calibration Mess

main.cpp

#include <Arduino.h>
#include <Wire.h>
#include "ota.h"
#include <WiFiManager.h>  //https://github.com/tzapu/WiFiManager WiFi Configuration Magic

#define TRACKER_ADDRESS 0x4A
#define AUX_ADDRESS 0x4B
#define TRACKER_INT 16
#define AUX_INT 13

#define USING_ADDRESS AUX_ADDRESS

#if USING_ADDRESS == TRACKER_ADDRESS
#define BNO08X_INT TRACKER_INT
#else
#define BNO08X_INT AUX_INT
#endif

// For SPI mode, we also need a RESET
// but not for I2C or UART
#define BNO08X_RESET -1

#define CLEARING true

#if CLEARING
#include <Adafruit_BNO08x.h>
Adafruit_BNO08x bno08x(BNO08X_RESET);
#else
#include "SparkFun_BNO080_Arduino_Library.h"  // Click here to get the library: http://librarymanager/All#SparkFun_BNO080
BNO080 myIMU;
#endif

void setup() {
    Serial.begin(115200);
    Serial.println("nullstalgia's BNO calibration mess!");
    WiFiManager wifiManager;

    wifiManager.setCaptivePortalEnable(true);

    wifiManager.setDarkMode(true);
    wifiManager.autoConnect("BNOTesting");
    Serial.println("Connected");
    Serial.println(WiFi.localIP().toString());
    OTA::otaSetup("SlimeVR-OTA");
    Serial.println();
    Serial.println("BNO080 Read Example");

    Wire.begin(14, 12);
    Wire.setClock(400000);  // Increase I2C data rate to 400kHz

#if CLEARING
    // Try to initialize!
    if (!bno08x.begin_I2C(USING_ADDRESS)) {
        // if (!bno08x.begin_UART(&Serial1)) {  // Requires a device with > 300
        // byte UART buffer! if (!bno08x.begin_SPI(BNO08X_CS, BNO08X_INT)) {
        Serial.println("Failed to find BNO08x chip");
        while (1) {
            delay(10);
            OTA::otaUpdate();
        }
    }
#else
    myIMU.begin(USING_ADDRESS, Wire, BNO08X_INT);
#endif
    Serial.println("BNO08x Found!");

#if CLEARING

#else
    // Enable dynamic calibration for accel, gyro, and mag
    myIMU.calibrateAll();  // Turn on cal for Accel, Gyro, and Mag

    // Enable Game Rotation Vector output
    myIMU.enableGameRotationVector(100);

    // Enable Magnetic Field output
    myIMU.enableMagnetometer(20);  // set to 50hz according to 1000-4044

    //myIMU.enableLinearAccelerometer(100);

    myIMU.enableAccelerometer(100);

    myIMU.enableGyro(100);

    // Once magnetic field is 2 or 3, run the Save DCD Now command
    Serial.println(F("Calibrating. Press 's' to save to flash"));
    Serial.println(F("Output in form x, y, z, in uTesla"));
#endif
}
// Given a accuracy number, print what it means
void printAccuracyLevel(byte accuracyNumber) {
    if (accuracyNumber == 0)
        Serial.print(F("Unreliable"));
    else if (accuracyNumber == 1)
        Serial.print(F("Low"));
    else if (accuracyNumber == 2)
        Serial.print(F("Medium"));
    else if (accuracyNumber == 3)
        Serial.print(F("High"));
}

void loop() {
    OTA::otaUpdate();
    if (Serial.available()) {
        byte incoming = Serial.read();

        if (incoming == 's') {
#if !CLEARING
            myIMU.saveCalibration();  // Saves the current dynamic calibration
                                      // data (DCD) to memory
            myIMU.requestCalibrationStatus();  // Sends command to get the
                                               // latest calibration status

            // Wait for calibration response, timeout if no response
            int counter = 100;
            while (1) {
                if (--counter == 0) break;
                if (myIMU.dataAvailable() == true) {
                    // The IMU can report many different things. We must wait
                    // for the ME Calibration Response Status byte to go to zero
                    if (myIMU.calibrationComplete() == true) {
                        Serial.println("Calibration data successfully stored");
                        delay(1000);
                        break;
                    }
                }

                delay(1);
            }
            if (counter == 0) {
                Serial.println(
                    "Calibration data failed to store. Please try again.");
            }

            // myIMU.endCalibration(); //Turns off all calibration
            // In general, calibration should be left on at all times. The
            // BNO080 auto-calibrates and auto-records cal data roughly every 5
            // minutes

#else
            Serial.println("Wrong mode!");
#endif
        }
        if (incoming == 'r') {
            #if CLEARING
            int success = sh2_clearDcdAndReset();
            Serial.print("Clear DCD and Reset: ");
            Serial.println((success == 0) ? "success" : "fail");
            delay(100);
            uint32_t newData = 0;
            success = sh2_setFrs(DYNAMIC_CALIBRATION, &newData, 0);
            Serial.print("Clear FSR Record: ");
            Serial.println((success == 0) ? "success" : "fail");
            delay(100);
            success = sh2_clearDcdAndReset();
            Serial.print("Clear DCD and Reset (again): ");
            Serial.println((success == 0) ? "success" : "fail");
            delay(100);/*
            sh2_CalStatus_t calstat;
            success = sh2_startCal();
            success = sh2_finishCal(&calstat);
            Serial.print("Stop Cal: ");
            Serial.println((success == 0) ? "success" : "fail");
            Serial.print("CalStat: ");
            Serial.println(calstat);*/
            #else
            Serial.println("Wrong mode!");
            #endif
        }
    }
    #if !CLEARING
    // Look for reports from the IMU
    if (myIMU.dataAvailable() == true) {
        float x = myIMU.getMagX();
        float y = myIMU.getMagY();
        float z = myIMU.getMagZ();
        byte magAccuracy = myIMU.getMagAccuracy();

        byte accelAccuracy;
        float accelX, accelY, accelZ;
        myIMU.getAccel(accelX, accelY, accelZ, accelAccuracy);

        byte gyroAccuracy = myIMU.getGyroAccuracy();
        //byte linAccelAccuracy = myIMU.getLinAccelAccuracy();
        //byte quatRatAccuracy = myIMU.getQuatRadianAccuracy();

        float x2 = myIMU.getGyroX();
        float y2 = myIMU.getGyroY();
        float z2 = myIMU.getGyroZ();

        Serial.print(x, 2);
        Serial.print(F(","));
        Serial.print(y, 2);
        Serial.print(F(","));
        Serial.print(z, 2);
        Serial.print(F(","));
        printAccuracyLevel(magAccuracy);
        Serial.print(F(","));

        Serial.print("\t");

        Serial.print(accelX, 2);
        Serial.print(F(","));
        Serial.print(accelY, 2);
        Serial.print(F(","));
        Serial.print(accelZ, 2);
        Serial.print(F(","));
        printAccuracyLevel(accelAccuracy);
        Serial.print(F(","));

        Serial.print("\t");

        Serial.print(x2, 2);
        Serial.print(F(","));
        Serial.print(y2, 2);
        Serial.print(F(","));
        Serial.print(z2, 2);
        Serial.print(F(","));
        printAccuracyLevel(gyroAccuracy);
        Serial.print(F(","));

        Serial.println();

/*
        Serial.print("magAccuracy: ");
        printAccuracyLevel(magAccuracy);
        //Serial.println();
        //Serial.print("quatAccuracy: ");
        //printAccuracyLevel(quatAccuracy);
        Serial.println();
        Serial.print("accelAccuracy: ");
        printAccuracyLevel(accelAccuracy);
        Serial.println();
        Serial.print("gyroAccuracy: ");
        printAccuracyLevel(gyroAccuracy);
        Serial.println();
        Serial.print("linAccelAccuracy: ");
        printAccuracyLevel(linAccelAccuracy);
        //Serial.println();
        //Serial.print("quatRatAccuracy: ");
        //printAccuracyLevel(quatRatAccuracy);
        Serial.println();

        Serial.println();
        delay(125);
        */
    }
    #endif
}