diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp index 5d9137776..c2d5c39fb 100644 --- a/lib/bno080/BNO080.cpp +++ b/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; diff --git a/lib/bno080/BNO080.h b/lib/bno080/BNO080.h index b508f183f..eee52a71b 100644 --- a/lib/bno080/BNO080.h +++ b/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 diff --git a/src/GlobalVars.h b/src/GlobalVars.h index 12e12e742..9e97163bb 100644 --- a/src/GlobalVars.h +++ b/src/GlobalVars.h @@ -28,10 +28,12 @@ #include "status/StatusManager.h" #include "configuration/Configuration.h" #include "sensors/SensorManager.h" +#include extern SlimeVR::LEDManager ledManager; extern SlimeVR::Status::StatusManager statusManager; extern SlimeVR::Configuration::Configuration configuration; extern SlimeVR::Sensors::SensorManager sensorManager; +extern Timer<> globalTimer; #endif diff --git a/src/main.cpp b/src/main.cpp index 623445432..8b993e0de 100644 --- a/src/main.cpp +++ b/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 #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()); diff --git a/src/network/packets.h b/src/network/packets.h index d3602d630..a44649d08 100644 --- a/src/network/packets.h +++ b/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 } diff --git a/src/network/udpclient.cpp b/src/network/udpclient.cpp index b7465cddb..24422ec82 100644 --- a/src/network/udpclient.cpp +++ b/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) { diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 4fee5cb56..35ee2160d 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/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); diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 28a754483..f5857cfda 100644 --- a/src/sensors/bno055sensor.cpp +++ b/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; } } diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 628f5e37e..d33a43344 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -117,19 +117,13 @@ void BNO080Sensor::motionLoop() #if USE_6_AXIS if (imu.hasNewGameQuat()) // New quaternion if context { - imu.getGameQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, calibrationAccuracy); - quaternion *= sensorOffset; + imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); + fusedRotation *= sensorOffset; - #if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } - #endif // ENABLE_INSPECTION - - if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newData = true; - lastQuatSent = quaternion; + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; } // Leave new quaternion if context open, it's closed later @@ -137,19 +131,13 @@ void BNO080Sensor::motionLoop() if (imu.hasNewQuat()) // New quaternion if context { - imu.getQuat(quaternion.x, quaternion.y, quaternion.z, quaternion.w, magneticAccuracyEstimate, calibrationAccuracy); - quaternion *= sensorOffset; + imu.getQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, magneticAccuracyEstimate, calibrationAccuracy); + fusedRotation *= sensorOffset; - #if ENABLE_INSPECTION + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } - #endif // ENABLE_INSPECTION - - if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) - { - newData = true; - lastQuatSent = quaternion; + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; } // Leave new quaternion if context open, it's closed later #endif // USE_6_AXIS @@ -158,7 +146,8 @@ void BNO080Sensor::motionLoop() #if SEND_ACCELERATION { uint8_t acc; - this->imu.getLinAccel(this->linearAcceleration[0], this->linearAcceleration[1], this->linearAcceleration[2], acc); + this->imu.getLinAccel(this->acceleration[0], this->acceleration[1], this->acceleration[2], acc); + this->newAcceleration = true; } #endif // SEND_ACCELERATION } // Closing new quaternion if context @@ -217,24 +206,28 @@ uint8_t BNO080Sensor::getSensorState() { void BNO080Sensor::sendData() { - if (newData) + if (newFusedRotation) { - newData = false; - Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); + newFusedRotation = false; + Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); + +#ifdef DEBUG_SENSOR + m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); +#endif + } #if SEND_ACCELERATION - Network::sendAccel(this->linearAcceleration, this->sensorId); + if(newAcceleration) + { + newAcceleration = false; + Network::sendAccel(this->acceleration, this->sensorId); + } #endif #if !USE_6_AXIS Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId); #endif -#ifdef DEBUG_SENSOR - m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(quaternion)); -#endif - } - #if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION if (newMagData) { @@ -253,18 +246,7 @@ void BNO080Sensor::sendData() void BNO080Sensor::startCalibration(int calibrationType) { - // TODO It only calibrates gyro, it should have multiple calibration modes, and check calibration status in motionLoop() - ledManager.pattern(20, 20, 10); - ledManager.blink(2000); - imu.calibrateGyro(); - do - { - ledManager.on(); - imu.requestCalibrationStatus(); - delay(20); - imu.getReadings(); - ledManager.off(); - delay(20); - } while (!imu.calibrationComplete()); - imu.saveCalibration(); + // BNO does automatic calibration, + // it's always enabled except accelerometer + // that is disabled 30 seconds after startup } \ No newline at end of file diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 0a0e40745..cc88c98e3 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -65,8 +65,6 @@ void ICM20948Sensor::motionLoop() } #endif - timer.tick(); - readFIFOToEnd(); readRotation(); checkSensorTimeout(); @@ -91,27 +89,28 @@ void ICM20948Sensor::readFIFOToEnd() void ICM20948Sensor::sendData() { - if(newData && lastDataSent + 7 < millis()) + if(newFusedRotation && lastDataSent + 7 < millis()) { lastDataSent = millis(); - newData = false; + newFusedRotation = false; #if(USE_6_AXIS) { - Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, 0, sensorId); + Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, 0, sensorId); } #else { - Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy, sensorId); + Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy, sensorId); } #endif + } - #if SEND_ACCELERATION - { - Network::sendAccel(linearAcceleration, sensorId); - } - #endif +#if SEND_ACCELERATION + if(newAcceleration) { + newAcceleration = false; + Network::sendAccel(acceleration, sensorId); } +#endif } void ICM20948Sensor::startCalibration(int calibrationType) @@ -123,7 +122,7 @@ void ICM20948Sensor::startCalibration(int calibrationType) void ICM20948Sensor::startCalibrationAutoSave() { #if SAVE_BIAS - timer.in(bias_save_periods[0] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->saveCalibration(true); return false; }, this); + globalTimer.in(bias_save_periods[0] * 1000, [](void *arg) -> bool { ((ICM20948Sensor*)arg)->saveCalibration(true); return false; }, this); #endif } @@ -319,24 +318,18 @@ void ICM20948Sensor::readRotation() double q2 = ((double)dmpData.Quat6.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 double q3 = ((double)dmpData.Quat6.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - quaternion.w = q0; - quaternion.x = q1; - quaternion.y = q2; - quaternion.z = q3; + fusedRotation.w = q0; + fusedRotation.x = q1; + fusedRotation.y = q2; + fusedRotation.z = q3; #if SEND_ACCELERATION - calculateAccelerationWithoutGravity(&quaternion); + calculateAccelerationWithoutGravity(&fusedRotation); #endif - quaternion *= sensorOffset; //imu rotation - - #if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } - #endif + fusedRotation *= sensorOffset; //imu rotation - newData = true; + newFusedRotation = true; lastData = millis(); } } @@ -363,12 +356,6 @@ void ICM20948Sensor::readRotation() quaternion *= sensorOffset; //imu rotation - #if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } - #endif - newData = true; lastData = millis(); } @@ -419,7 +406,7 @@ void ICM20948Sensor::saveCalibration(bool repeat) bias_save_counter++; // Possible: Could make it repeat the final timer value if any of the biases are still 0. Save strategy could be improved. if (sizeof(bias_save_periods) != bias_save_counter) { - timer.in( + globalTimer.in( bias_save_periods[bias_save_counter] * 1000, [](void* arg) -> bool { ((ICM20948Sensor*)arg)->saveCalibration(true); @@ -484,9 +471,9 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) { if((dmpData.header & DMP_header_bitmap_Accel) > 0) { - this->linearAcceleration[0] = (float)this->dmpData.Raw_Accel.Data.X; - this->linearAcceleration[1] = (float)this->dmpData.Raw_Accel.Data.Y; - this->linearAcceleration[2] = (float)this->dmpData.Raw_Accel.Data.Z; + this->acceleration[0] = (float)this->dmpData.Raw_Accel.Data.X; + this->acceleration[1] = (float)this->dmpData.Raw_Accel.Data.Y; + this->acceleration[2] = (float)this->dmpData.Raw_Accel.Data.Z; // get the component of the acceleration that is gravity float gravity[3]; @@ -495,14 +482,15 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) gravity[2] = quaternion->w * quaternion->w - quaternion->x * quaternion->x - quaternion->y * quaternion->y + quaternion->z * quaternion->z; // subtract gravity from the acceleration vector - this->linearAcceleration[0] -= gravity[0] * ACCEL_SENSITIVITY_4G; - this->linearAcceleration[1] -= gravity[1] * ACCEL_SENSITIVITY_4G; - this->linearAcceleration[2] -= gravity[2] * ACCEL_SENSITIVITY_4G; + this->acceleration[0] -= gravity[0] * ACCEL_SENSITIVITY_4G; + this->acceleration[1] -= gravity[1] * ACCEL_SENSITIVITY_4G; + this->acceleration[2] -= gravity[2] * ACCEL_SENSITIVITY_4G; // finally scale the acceleration values to mps2 - this->linearAcceleration[0] *= ASCALE_4G; - this->linearAcceleration[1] *= ASCALE_4G; - this->linearAcceleration[2] *= ASCALE_4G; + this->acceleration[0] *= ASCALE_4G; + this->acceleration[1] *= ASCALE_4G; + this->acceleration[2] *= ASCALE_4G; + this->newAcceleration = true; } } #endif diff --git a/src/sensors/icm20948sensor.h b/src/sensors/icm20948sensor.h index afa33b972..da9dfb140 100644 --- a/src/sensors/icm20948sensor.h +++ b/src/sensors/icm20948sensor.h @@ -24,8 +24,7 @@ #define SLIMEVR_ICM20948SENSOR_H_ #include -#include "sensor.h" -#include // Used for periodically saving bias +#include "sensor.h" // Used for periodically saving bias class ICM20948Sensor : public Sensor { @@ -69,8 +68,6 @@ class ICM20948Sensor : public Sensor void readFIFOToEnd(); #define OVERRIDEDMPSETUP true - - Timer<> timer = timer_create_default(); // TapDetector tapDetector; }; diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index 8c18a0cc0..e90b1053f 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -137,8 +137,8 @@ void MPU6050Sensor::motionLoop() if (imu.dmpGetCurrentFIFOPacket(fifoBuffer)) { imu.dmpGetQuaternion(&rawQuat, fifoBuffer); - quaternion.set(-rawQuat.y, rawQuat.x, rawQuat.z, rawQuat.w); - quaternion *= sensorOffset; + fusedRotation.set(-rawQuat.y, rawQuat.x, rawQuat.z, rawQuat.w); + fusedRotation *= sensorOffset; #if SEND_ACCELERATION { @@ -156,22 +156,17 @@ void MPU6050Sensor::motionLoop() this->imu.dmpGetLinearAccel(&this->rawAccel, &this->rawAccel, &gravity); // convert acceleration to m/s^2 (implicitly casts to float) - this->linearAcceleration[0] = this->rawAccel.x * ASCALE_2G; - this->linearAcceleration[1] = this->rawAccel.y * ASCALE_2G; - this->linearAcceleration[2] = this->rawAccel.z * ASCALE_2G; + this->acceleration[0] = this->rawAccel.x * ASCALE_2G; + this->acceleration[1] = this->rawAccel.y * ASCALE_2G; + this->acceleration[2] = this->rawAccel.z * ASCALE_2G; + this->newAcceleration = true; } #endif - -#if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } -#endif - if (!OPTIMIZE_UPDATES || !lastQuatSent.equalsWithEpsilon(quaternion)) + if (!OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newData = true; - lastQuatSent = quaternion; + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; } } } diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 8f391c1f0..976fadc87 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -198,13 +198,14 @@ void MPU9250Sensor::motionLoop() { this->imu.dmpGetLinearAccel(&this->rawAccel, &this->rawAccel, &grav); // convert acceleration to m/s^2 (implicitly casts to float) - this->linearAcceleration[0] = this->rawAccel.x * ASCALE_2G; - this->linearAcceleration[1] = this->rawAccel.y * ASCALE_2G; - this->linearAcceleration[2] = this->rawAccel.z * ASCALE_2G; + this->acceleration[0] = this->rawAccel.x * ASCALE_2G; + this->acceleration[1] = this->rawAccel.y * ASCALE_2G; + this->acceleration[2] = this->rawAccel.z * ASCALE_2G; + this->newAcceleration = true; } #endif - quaternion = correction * quat; + fusedRotation = correction * quat; #else union fifo_sample_raw buf; @@ -232,17 +233,11 @@ void MPU9250Sensor::motionLoop() { quaternion.set(-q[2], q[1], q[3], q[0]); #endif - quaternion *= sensorOffset; + fusedRotation *= sensorOffset; -#if ENABLE_INSPECTION - { - Network::sendInspectionFusedIMUData(sensorId, quaternion); - } -#endif - - if(!lastQuatSent.equalsWithEpsilon(quaternion)) { - newData = true; - lastQuatSent = quaternion; + if(!lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; } } diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 147ce825e..91c7f93e7 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -30,20 +30,21 @@ uint8_t Sensor::getSensorState() { } void Sensor::sendData() { - if(newData) { - newData = false; - Network::sendRotationData(&quaternion, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); - -#if SEND_ACCELERATION - { - Network::sendAccel(linearAcceleration, sensorId); - } -#endif + if(newFusedRotation) { + newFusedRotation = false; + Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(quaternion)); #endif } + +#if SEND_ACCELERATION + if(newAcceleration) { + newAcceleration = false; + Network::sendAccel(acceleration, sensorId); + } +#endif } void Sensor::printTemperatureCalibrationUnsupported() { diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index b6b09994b..25b535e8b 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -67,8 +67,8 @@ class Sensor uint8_t getSensorType() { return sensorType; }; - Quat& getQuaternion() { - return quaternion; + Quat& getFusedRotation() { + return fusedRotation; }; bool hadData = false; @@ -77,15 +77,16 @@ class Sensor uint8_t sensorId = 0; uint8_t sensorType = 0; bool configured = false; - bool newData = false; bool working = false; uint8_t calibrationAccuracy = 0; Quat sensorOffset; - Quat quaternion{}; - Quat lastQuatSent{}; + bool newFusedRotation = false; + Quat fusedRotation{}; + Quat lastFusedRotationSent{}; - float linearAcceleration[3]{}; + bool newAcceleration = false; + float acceleration[3]{}; SlimeVR::Logging::Logger m_Logger; diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index bfb906398..cd7095bb2 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -71,14 +71,14 @@ namespace SerialCommands { logger.info( "Sensor 1: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s", getIMUNameByType(sensor1->getSensorType()), - UNPACK_QUATERNION(sensor1->getQuaternion()), + UNPACK_QUATERNION(sensor1->getFusedRotation()), 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()), + UNPACK_QUATERNION(sensor2->getFusedRotation()), sensor2->isWorking() ? "true" : "false", sensor2->hadData ? "true" : "false" ); @@ -150,7 +150,7 @@ namespace SerialCommands { logger.info( "[TEST] Sensor 1: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s", getIMUNameByType(sensor1->getSensorType()), - UNPACK_QUATERNION(sensor1->getQuaternion()), + UNPACK_QUATERNION(sensor1->getFusedRotation()), sensor1->isWorking() ? "true" : "false", sensor1->hadData ? "true" : "false" );