Minor firmware refactoring

lib/bno080/BNO080.cpp

@@ -1270,39 +1270,9 @@ void BNO080::enableActivityClassifier(uint16_t timeBetweenReports, uint32_t acti
 	setFeatureCommand(SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER, timeBetweenReports, activitiesToEnable);
 }
 
-//Sends the commands to begin calibration of the accelerometer
-void BNO080::calibrateAccelerometer()
-{
-	sendCalibrateCommand(CALIBRATE_ACCEL);
-}
-
-//Sends the commands to begin calibration of the gyro
-void BNO080::calibrateGyro()
-{
-	sendCalibrateCommand(CALIBRATE_GYRO);
-}
-
-//Sends the commands to begin calibration of the magnetometer
-void BNO080::calibrateMagnetometer()
-{
-	sendCalibrateCommand(CALIBRATE_MAG);
-}
-
-//Sends the commands to begin calibration of the planar accelerometer
-void BNO080::calibratePlanarAccelerometer()
-{
-	sendCalibrateCommand(CALIBRATE_PLANAR_ACCEL);
-}
-
-//See 2.2 of the Calibration Procedure document 1000-4044
-void BNO080::calibrateAll()
-{
-	sendCalibrateCommand(CALIBRATE_ACCEL_GYRO_MAG);
-}
-
 void BNO080::endCalibration()
 {
-	sendCalibrateCommand(CALIBRATE_STOP); //Disables all calibrations
+	sendCalibrateCommand(0); //Disables all calibrations
 }
 
 //See page 51 of reference manual - ME Calibration Response
@@ -1374,37 +1344,34 @@ void BNO080::sendCommand(uint8_t command)
 
 //This tells the BNO080 to begin calibrating
 //See page 50 of reference manual and the 1000-4044 calibration doc
+//The argument is a set of binary flags see SH2_CAL_ACCEL
 void BNO080::sendCalibrateCommand(uint8_t thingToCalibrate)
 {
-	/*shtpData[3] = 0; //P0 - Accel Cal Enable
-	shtpData[4] = 0; //P1 - Gyro Cal Enable
+	/*
+	shtpData[3] = 0; //P0 - Accel Cal Enable
+	shtpData[4] = 0; //P1 - Gyro In-Hand Cal Enable
 	shtpData[5] = 0; //P2 - Mag Cal Enable
 	shtpData[6] = 0; //P3 - Subcommand 0x00
 	shtpData[7] = 0; //P4 - Planar Accel Cal Enable
-	shtpData[8] = 0; //P5 - Reserved
+	shtpData[8] = 0; //P5 - Gyro On-Table Cal Enable
 	shtpData[9] = 0; //P6 - Reserved
 	shtpData[10] = 0; //P7 - Reserved
-	shtpData[11] = 0; //P8 - Reserved*/
+	shtpData[11] = 0; //P8 - Reserved
+	*/
 
 	for (uint8_t x = 3; x < 12; x++) //Clear this section of the shtpData array
 		shtpData[x] = 0;
 
-	if (thingToCalibrate == CALIBRATE_ACCEL)
+	if ((thingToCalibrate & SH2_CAL_ACCEL) > 1)
 		shtpData[3] = 1;
-	else if (thingToCalibrate == CALIBRATE_GYRO)
+	if ((thingToCalibrate & SH2_CAL_GYRO_IN_HAND) > 1)
 		shtpData[4] = 1;
-	else if (thingToCalibrate == CALIBRATE_MAG)
+	if ((thingToCalibrate & SH2_CAL_MAG) > 1)
 		shtpData[5] = 1;
-	else if (thingToCalibrate == CALIBRATE_PLANAR_ACCEL)
+	if ((thingToCalibrate & SH2_CAL_PLANAR) > 1)
 		shtpData[7] = 1;
-	else if (thingToCalibrate == CALIBRATE_ACCEL_GYRO_MAG)
-	{
-		shtpData[3] = 1;
-		shtpData[4] = 1;
-		shtpData[5] = 1;
-	}
-	else if (thingToCalibrate == CALIBRATE_STOP)
-		; //Do nothing, bytes are set to zero
+	if ((thingToCalibrate & SH2_CAL_ON_TABLE) > 1)
+		shtpData[8] = 1;
 
 	//Make the internal calStatus variable non-zero (operation failed) so that user can test while we wait
 	calibrationStatus = 1;

lib/bno080/BNO080.h

@@ -129,12 +129,11 @@ const byte CHANNEL_GYRO = 5;
 #define COMMAND_OSCILLATOR 10
 #define COMMAND_CLEAR_DCD 11
 
-#define CALIBRATE_ACCEL 0
-#define CALIBRATE_GYRO 1
-#define CALIBRATE_MAG 2
-#define CALIBRATE_PLANAR_ACCEL 3
-#define CALIBRATE_ACCEL_GYRO_MAG 4
-#define CALIBRATE_STOP 5
+#define SH2_CAL_ACCEL (0x01)
+#define SH2_CAL_GYRO_IN_HAND  (0x02)
+#define SH2_CAL_MAG   (0x04)
+#define SH2_CAL_PLANAR (0x08)
+#define SH2_CAL_ON_TABLE (0x10)
 
 #define MAX_PACKET_SIZE 128 //Packets can be up to 32k but we don't have that much RAM.
 #define MAX_METADATA_SIZE 9 //This is in words. There can be many but we mostly only care about the first 9 (Qs, range, etc)
@@ -238,12 +237,7 @@ class BNO080
 	float getMagY();
 	float getMagZ();
 	uint8_t getMagAccuracy();
-
-	void calibrateAccelerometer();
-	void calibrateGyro();
-	void calibrateMagnetometer();
-	void calibratePlanarAccelerometer();
-	void calibrateAll();
+
 	void endCalibration();
 	void saveCalibration();
 	void requestCalibrationStatus(); //Sends command to get status

src/GlobalVars.h

@@ -28,10 +28,12 @@
 #include "status/StatusManager.h"
 #include "configuration/Configuration.h"
 #include "sensors/SensorManager.h"
+#include <arduino-timer.h>
 
 extern SlimeVR::LEDManager ledManager;
 extern SlimeVR::Status::StatusManager statusManager;
 extern SlimeVR::Configuration::Configuration configuration;
 extern SlimeVR::Sensors::SensorManager sensorManager;
+extern Timer<> globalTimer;
 
 #endif

src/main.cpp

@@ -23,15 +23,12 @@
 
 #include "Wire.h"
 #include "ota.h"
-#include "sensors/SensorManager.h"
-#include "configuration/Configuration.h"
+#include "GlobalVars.h"
 #include "network/network.h"
 #include "globals.h"
 #include "credentials.h"
 #include <i2cscan.h>
 #include "serial/serialcommands.h"
-#include "LEDManager.h"
-#include "status/StatusManager.h"
 #include "batterymonitor.h"
 #include "logging/Logger.h"
 
@@ -40,6 +37,7 @@ SlimeVR::Sensors::SensorManager sensorManager;
 SlimeVR::LEDManager ledManager(LED_PIN);
 SlimeVR::Status::StatusManager statusManager;
 SlimeVR::Configuration::Configuration configuration;
+Timer<> globalTimer;
 
 int sensorToCalibrate = -1;
 bool blinking = false;
@@ -52,6 +50,7 @@ BatteryMonitor battery;
 void setup()
 {
     Serial.begin(serialBaudRate);
+    globalTimer = timer_create_default();
 
 #ifdef ESP32C3 
     // Wait for the Computer to be able to connect.
@@ -111,6 +110,7 @@ void setup()
 
 void loop()
 {
+    globalTimer.tick();
     SerialCommands::update();
     OTA::otaUpdate();
     Network::update(sensorManager.getFirst(), sensorManager.getSecond());

src/network/packets.h

@@ -105,10 +105,6 @@ namespace Network {
 #if ENABLE_INSPECTION
     void sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA);
     void sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA);
-
-    void sendInspectionFusedIMUData(uint8_t sensorId, Quat quaternion);
-
-    void sendInspectionCorrectionData(uint8_t sensorId, Quat quaternion);
 #endif
 }
 

src/network/udpclient.cpp

@@ -479,70 +479,6 @@ void Network::sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, flo
         udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError());
     }
 }
-
-void Network::sendInspectionFusedIMUData(uint8_t sensorId, Quat quaternion)
-{
-    if (!connected) 
-    {
-        return;
-    }
-
-    if (!DataTransfer::beginPacket())
-    {
-        udpClientLogger.error("FusedIMUData write begin error: %d", Udp.getWriteError());
-        return;
-    }
-
-    DataTransfer::sendPacketType(PACKET_INSPECTION);
-    DataTransfer::sendPacketNumber();
-
-    DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_FUSED_IMU_DATA);
-
-    DataTransfer::sendByte(sensorId);
-    DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
-
-    DataTransfer::sendFloat(quaternion.x);
-    DataTransfer::sendFloat(quaternion.y);
-    DataTransfer::sendFloat(quaternion.z);
-    DataTransfer::sendFloat(quaternion.w);
-
-    if(!DataTransfer::endPacket())
-    {
-        udpClientLogger.error("FusedIMUData write end error: %d", Udp.getWriteError());
-    }
-}
-
-void Network::sendInspectionCorrectionData(uint8_t sensorId, Quat quaternion)
-{
-    if (!connected) 
-    {
-        return;
-    }
-
-    if (!DataTransfer::beginPacket())
-    {
-        udpClientLogger.error("CorrectionData write begin error: %d", Udp.getWriteError());
-        return;
-    }
-
-    DataTransfer::sendPacketType(PACKET_INSPECTION);
-    DataTransfer::sendPacketNumber();
-
-    DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_CORRECTION_DATA);
-
-    DataTransfer::sendByte(sensorId);
-    DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT);
-
-    DataTransfer::sendFloat(quaternion.x);
-    DataTransfer::sendFloat(quaternion.y);
-    DataTransfer::sendFloat(quaternion.z);
-    DataTransfer::sendFloat(quaternion.w);
-
-    if(!DataTransfer::endPacket())
-    {
-        udpClientLogger.error("CorrectionData write end error: %d", Udp.getWriteError());
-    }
-}
 #endif
 
 void returnLastPacket(int len) {

src/sensors/bmi160sensor.cpp

@@ -400,9 +400,9 @@ void BMI160Sensor::motionLoop() {
                 return;
             }
 
-            quaternion.set(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
+            fusedRotation.set(qwxyz[1], qwxyz[2], qwxyz[3], qwxyz[0]);
 
-            const Quat q = quaternion;
+            const Quat q = fusedRotation;
             sensor_real_t vecGravity[3];
             vecGravity[0] = 2 * (q.x * q.z - q.w * q.y);
             vecGravity[1] = 2 * (q.w * q.x + q.y * q.z);
@@ -413,22 +413,17 @@ void BMI160Sensor::motionLoop() {
             linAccel.y = lastAxyz[1] - vecGravity[1] * CONST_EARTH_GRAVITY;
             linAccel.z = lastAxyz[2] - vecGravity[2] * CONST_EARTH_GRAVITY;
 
-            linearAcceleration[0] = linAccel.x;
-            linearAcceleration[1] = linAccel.y;
-            linearAcceleration[2] = linAccel.z;
+            acceleration[0] = linAccel.x;
+            acceleration[1] = linAccel.y;
+            acceleration[2] = linAccel.z;
+            newAcceleration = true;
 
-            quaternion *= sensorOffset;
+            fusedRotation *= sensorOffset;
 
-            #if ENABLE_INSPECTION
+            if (!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation))
             {
-                Network::sendInspectionFusedIMUData(sensorId, quaternion);
-            }
-            #endif
-
-            if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion))
-            {
-                newData = true;
-                lastQuatSent = quaternion;
+                newFusedRotation = true;
+                lastFusedRotationSent = fusedRotation;
             }
 
             optimistic_yield(100);

src/sensors/bno055sensor.cpp

@@ -62,27 +62,22 @@ void BNO055Sensor::motionLoop() {
 
     // TODO Optimize a bit with setting rawQuat directly
     Quat quat = imu.getQuat();
-    quaternion.set(quat.x, quat.y, quat.z, quat.w);
-    quaternion *= sensorOffset;
+    fusedRotation.set(quat.x, quat.y, quat.z, quat.w);
+    fusedRotation *= sensorOffset;
 
 #if SEND_ACCELERATION
     {
         Vector3 accel = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
-        this->linearAcceleration[0] = accel.x;
-        this->linearAcceleration[1] = accel.y;
-        this->linearAcceleration[2] = accel.z;
+        this->acceleration[0] = accel.x;
+        this->acceleration[1] = accel.y;
+        this->acceleration[2] = accel.z;
+        this->newAcceleration = true;
     }
 #endif
 
-#if ENABLE_INSPECTION
-    {
-        Network::sendInspectionFusedIMUData(sensorId, quaternion);
-    }
-#endif
-
-    if(!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) {
-        newData = true;
-        lastQuatSent = quaternion;
+    if(!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) {
+        newFusedRotation = true;
+        lastFusedRotationSent = fusedRotation;
     }
 }