From c2668adc4970fdc186c633a95af13a6ff0f9c808 Mon Sep 17 00:00:00 2001 From: nekomona Date: Thu, 28 Mar 2024 22:52:11 +0800 Subject: [PATCH] Merge code for applying `sensorOffset` and setting data ready flag (#314) Merge applying of sensorOffset and data ready flag Co-authored-by: nekomona --- src/sensors/bmi160sensor.cpp | 9 ++------- src/sensors/bno055sensor.cpp | 10 ++-------- src/sensors/bno080sensor.cpp | 17 +++++++++-------- src/sensors/icm20948sensor.cpp | 25 +++++++------------------ src/sensors/icm42688sensor.cpp | 7 ++----- src/sensors/mpu6050sensor.cpp | 7 ++----- src/sensors/mpu9250sensor.cpp | 18 +++--------------- src/sensors/sensor.cpp | 6 ++++-- src/sensors/sensor.h | 4 ++-- 9 files changed, 33 insertions(+), 70 deletions(-) diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 52ef90ec8..17864bdc6 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -361,13 +361,8 @@ void BMI160Sensor::motionLoop() { if (elapsed >= sendInterval) { lastRotationPacketSent = now - (elapsed - sendInterval); - fusedRotation = sfusion.getQuaternionQuat(); - setFusedRotationReady(); - - acceleration = sfusion.getLinearAccVec(); - setAccelerationReady(); - - fusedRotation *= sensorOffset; + setFusedRotation(sfusion.getQuaternionQuat()); + setAcceleration(sfusion.getLinearAccVec()); optimistic_yield(100); } diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index 9161511e9..92b8baf87 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -60,16 +60,10 @@ void BNO055Sensor::motionLoop() { #endif // TODO Optimize a bit with setting rawQuat directly - Quat quat = imu.getQuat(); - fusedRotation.set(quat.x, quat.y, quat.z, quat.w); - fusedRotation *= sensorOffset; - setFusedRotationReady(); + setFusedRotation(imu.getQuat()); #if SEND_ACCELERATION - { - acceleration = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); - setAccelerationReady(); - } + setAcceleration(imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL)); #endif } diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index 2b9717164..7d7c0c2cf 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -116,20 +116,20 @@ void BNO080Sensor::motionLoop() #if USE_6_AXIS if (imu.hasNewGameQuat()) // New quaternion if context { - imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); - fusedRotation *= sensorOffset; + Quat nRotation; + imu.getGameQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, calibrationAccuracy); - setFusedRotationReady(); + setFusedRotation(nRotation); // Leave new quaternion if context open, it's closed later #else // USE_6_AXIS if (imu.hasNewQuat()) // New quaternion if context { - imu.getQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, magneticAccuracyEstimate, calibrationAccuracy); - fusedRotation *= sensorOffset; + Quat nRotation; + imu.getQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, magneticAccuracyEstimate, calibrationAccuracy); - setFusedRotationReady(); + setFusedRotation(nRotation); // Leave new quaternion if context open, it's closed later #endif // USE_6_AXIS @@ -137,8 +137,9 @@ void BNO080Sensor::motionLoop() #if SEND_ACCELERATION { uint8_t acc; - imu.getLinAccel(acceleration.x, acceleration.y, acceleration.z, acc); - setAccelerationReady(); + Vector3 nAccel; + imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc); + setAcceleration(nAccel); } #endif // SEND_ACCELERATION } // Closing new quaternion if context diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index 83c42db90..2eb3f092d 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -347,18 +347,13 @@ 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))); - fusedRotation.w = q0; - fusedRotation.x = q1; - fusedRotation.y = q2; - fusedRotation.z = q3; + Quat nRotation(q1, q2, q3, q0); // x, y, z, w #if SEND_ACCELERATION - calculateAccelerationWithoutGravity(&fusedRotation); + calculateAccelerationWithoutGravity(&nRotation); #endif - fusedRotation *= sensorOffset; //imu rotation - - newFusedRotation = true; + setFusedRotation(nRotation); lastData = millis(); } } @@ -374,18 +369,13 @@ void ICM20948Sensor::readRotation() double q2 = ((double)dmpData.Quat9.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 double q3 = ((double)dmpData.Quat9.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30 double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3))); - fusedRotation.w = q0; - fusedRotation.x = q1; - fusedRotation.y = q2; - fusedRotation.z = q3; + Quat nRotation(q1, q2, q3, q0); // x, y, z, w #if SEND_ACCELERATION - calculateAccelerationWithoutGravity(&fusedRotation); + calculateAccelerationWithoutGravity(&nRotation); #endif - fusedRotation *= sensorOffset; //imu rotation - - newFusedRotation = true; + setFusedRotation(nRotation); lastData = millis(); } } @@ -508,8 +498,7 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion) }; sfusion.updateAcc(Axyz); - acceleration = sfusion.getLinearAccVec(); - this->newAcceleration = true; + setAcceleration(sfusion.getLinearAccVec()); } } #endif diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp index 6884ba2a4..8bc017325 100644 --- a/src/sensors/icm42688sensor.cpp +++ b/src/sensors/icm42688sensor.cpp @@ -172,11 +172,8 @@ void ICM42688Sensor::motionLoop() { if (magExists) sfusion.updateMag(Mxyz); - fusedRotation = sfusion.getQuaternionQuat(); - fusedRotation *= sensorOffset; - acceleration = sfusion.getLinearAccVec(); - setFusedRotationReady(); - setAccelerationReady(); + setFusedRotation(sfusion.getQuaternionQuat()); + setAcceleration(sfusion.getLinearAccVec()); } void ICM42688Sensor::accel_read() { diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index e327ebd88..f3691ea94 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -139,9 +139,7 @@ void MPU6050Sensor::motionLoop() sfusion.updateQuaternion(rawQuat); - fusedRotation = sfusion.getQuaternionQuat(); - fusedRotation *= sensorOffset; - setFusedRotationReady(); + setFusedRotation(sfusion.getQuaternionQuat()); #if SEND_ACCELERATION { @@ -153,8 +151,7 @@ void MPU6050Sensor::motionLoop() sfusion.updateAcc(Axyz); - acceleration = sfusion.getLinearAccVec(); - setAccelerationReady(); + setAcceleration(sfusion.getLinearAccVec()); } #endif } diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 4419e5fda..85db8db25 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -162,9 +162,6 @@ void MPU9250Sensor::motionLoop() { parseMagData(temp); sfusion.updateMag(Mxyz); - - fusedRotation = sfusion.getQuaternionQuat(); - #if SEND_ACCELERATION { int16_t atemp[3]; @@ -172,12 +169,8 @@ void MPU9250Sensor::motionLoop() { parseAccelData(atemp); sfusion.updateAcc(Axyz); - - acceleration = sfusion.getLinearAccVec(); - setAccelerationReady(); } #endif - #else union fifo_sample_raw buf; uint16_t remaining_samples; @@ -196,16 +189,11 @@ void MPU9250Sensor::motionLoop() { sfusion.update9D(Axyz, Gxyz, Mxyz); } - - fusedRotation = sfusion.getQuaternionQuat(); - +#endif + setFusedRotation(sfusion.getQuaternionQuat()); #if SEND_ACCELERATION - acceleration = sfusion.getLinearAccVec(); - setAccelerationReady(); + setAcceleration(sfusion.getLinearAccVec()); #endif -#endif - fusedRotation *= sensorOffset; - setFusedRotationReady(); } void MPU9250Sensor::startCalibration(int calibrationType) { diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 0ce5c1145..9db9e9a8d 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -29,11 +29,13 @@ SensorStatus Sensor::getSensorState() { return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR; } -void Sensor::setAccelerationReady() { +void Sensor::setAcceleration(Vector3 a) { + acceleration = a; newAcceleration = true; } -void Sensor::setFusedRotationReady() { +void Sensor::setFusedRotation(Quat r) { + fusedRotation = r * sensorOffset; bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true; if (ENABLE_INSPECTION || changed) { newFusedRotation = true; diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index da6ce90b4..6d603a8c8 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -58,8 +58,8 @@ class Sensor virtual void postSetup(){}; virtual void motionLoop(){}; virtual void sendData(); - virtual void setAccelerationReady(); - virtual void setFusedRotationReady(); + virtual void setAcceleration(Vector3 a); + virtual void setFusedRotation(Quat r); virtual void startCalibration(int calibrationType){}; virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState();