Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merge code for applying sensorOffset and setting data ready flag #314

Merged
merged 1 commit into from
Mar 28, 2024
Merged

Merge code for applying sensorOffset and setting data ready flag #314

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 2 additions & 7 deletions src/sensors/bmi160sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand Down
10 changes: 2 additions & 8 deletions src/sensors/bno055sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
}

Expand Down
17 changes: 9 additions & 8 deletions src/sensors/bno080sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -116,29 +116,30 @@ 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

// Continuation of the new quaternion if context, used for both 6 and 9 axis
#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
Expand Down
25 changes: 7 additions & 18 deletions src/sensors/icm20948sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
}
Expand All @@ -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();
}
}
Expand Down Expand Up @@ -508,8 +498,7 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion)
};
sfusion.updateAcc(Axyz);

acceleration = sfusion.getLinearAccVec();
this->newAcceleration = true;
setAcceleration(sfusion.getLinearAccVec());
}
}
#endif
Expand Down
7 changes: 2 additions & 5 deletions src/sensors/icm42688sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down
7 changes: 2 additions & 5 deletions src/sensors/mpu6050sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,9 +139,7 @@ void MPU6050Sensor::motionLoop()

sfusion.updateQuaternion(rawQuat);

fusedRotation = sfusion.getQuaternionQuat();
fusedRotation *= sensorOffset;
setFusedRotationReady();
setFusedRotation(sfusion.getQuaternionQuat());

#if SEND_ACCELERATION
{
Expand All @@ -153,8 +151,7 @@ void MPU6050Sensor::motionLoop()

sfusion.updateAcc(Axyz);

acceleration = sfusion.getLinearAccVec();
setAccelerationReady();
setAcceleration(sfusion.getLinearAccVec());
}
#endif
}
Expand Down
18 changes: 3 additions & 15 deletions src/sensors/mpu9250sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,22 +162,15 @@ void MPU9250Sensor::motionLoop() {
parseMagData(temp);

sfusion.updateMag(Mxyz);

fusedRotation = sfusion.getQuaternionQuat();

#if SEND_ACCELERATION
{
int16_t atemp[3];
this->imu.dmpGetAccel(atemp, dmpPacket);
parseAccelData(atemp);

sfusion.updateAcc(Axyz);

acceleration = sfusion.getLinearAccVec();
setAccelerationReady();
}
#endif

#else
union fifo_sample_raw buf;
uint16_t remaining_samples;
Expand All @@ -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) {
Expand Down
6 changes: 4 additions & 2 deletions src/sensors/sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions src/sensors/sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
Loading