rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

OP-1750 - don't reuse invBmag, calculate mag magnitude only if mag samples are provided

Browse Source
This commit is contained in:
Alessio Morale 2015-03-20 21:45:50 +01:00
parent 4e9d092b03
commit 0d1b3ffb3d
1 changed files with 21 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
flight/libraries/insgps13state.c
View File

@ -376,35 +376,39 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
{ {
float Z[10] = { 0 }; float Z[10] = { 0 };
float Y[10] = { 0 }; float Y[10] = { 0 };
float invBmag;
// GPS Position in meters and in local NED frame // GPS Position in meters and in local NED frame
Z[0] = Pos[0]; Z[0] = Pos[0];
Z[1] = Pos[1]; Z[1] = Pos[1];
Z[2] = Pos[2]; Z[2] = Pos[2];
// GPS Velocity in meters and in local NED frame // GPS Velocity in meters and in local NED frame
Z[3] = Vel[0]; Z[3] = Vel[0];
Z[4] = Vel[1]; Z[4] = Vel[1];
Z[5] = Vel[2]; Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame // magnetometer data in any units (use unit vector) and in body frame
invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] * invBmag;
Z[7] = mag_data[1] * invBmag; if (SensorsUsed & MAG_SENSORS) {
Z[8] = mag_data[2] * invBmag; float invBmag = fast_invsqrtf(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] + mag_data[2] * mag_data[2]);
Z[6] = mag_data[0] * invBmag;
Z[7] = mag_data[1] * invBmag;
Z[8] = mag_data[2] * invBmag;
}
// barometric altimeter in meters and in local NED frame // barometric altimeter in meters and in local NED frame
Z[9] = BaroAlt; Z[9] = BaroAlt;
// EKF correction step // EKF correction step
LinearizeH(ekf.X, ekf.Be, ekf.H); LinearizeH(ekf.X, ekf.Be, ekf.H);
MeasurementEq(ekf.X, ekf.Be, Y); MeasurementEq(ekf.X, ekf.Be, Y);
SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed); SerialUpdate(ekf.H, ekf.R, Z, Y, ekf.P, ekf.X, SensorsUsed);
invBmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]); float invqmag = fast_invsqrtf(ekf.X[6] * ekf.X[6] + ekf.X[7] * ekf.X[7] + ekf.X[8] * ekf.X[8] + ekf.X[9] * ekf.X[9]);
ekf.X[6] *= invBmag; ekf.X[6] *= invqmag;
ekf.X[7] *= invBmag; ekf.X[7] *= invqmag;
ekf.X[8] *= invBmag; ekf.X[8] *= invqmag;
ekf.X[9] *= invBmag; ekf.X[9] *= invqmag;
// Update Nav solution structure // Update Nav solution structure
Nav.Pos[0] = ekf.X[0]; Nav.Pos[0] = ekf.X[0];
Nav.Pos[1] = ekf.X[1]; Nav.Pos[1] = ekf.X[1];