rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Do not use the data from the magnetometer if it contains NAN

Browse Source
This commit is contained in:
James Cotton 2012-07-21 11:43:31 -05:00
parent c5904b4667
commit 797a4def6a
1 changed files with 22 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
flight/Modules/Attitude/revolution/attitude.c
View File

@ -358,28 +358,30 @@ static int32_t updateAttitudeComplimentary(bool first_run)
float brot[3]; float brot[3];
float Rbe[3][3]; float Rbe[3][3];
MagnetometerData mag; MagnetometerData mag;
HomeLocationData home;
Quaternion2R(q, Rbe); Quaternion2R(q, Rbe);
MagnetometerGet(&mag); MagnetometerGet(&mag);
HomeLocationGet(&home);
rot_mult(Rbe, home.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); // If the mag is producing bad data don't use it (normally bad calibration)
mag.x /= mag_len; if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
mag.y /= mag_len; rot_mult(Rbe, home.Be, brot);
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
brot[0] /= bmag; mag.x /= mag_len;
brot[1] /= bmag; mag.y /= mag_len;
brot[2] /= bmag; mag.z /= mag_len;
// Only compute if neither vector is null float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
if (bmag < 1 || mag_len < 1) brot[0] /= bmag;
mag_err[0] = mag_err[1] = mag_err[2] = 0; brot[1] /= bmag;
else brot[2] /= bmag;
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else { } else {
mag_err[0] = mag_err[1] = mag_err[2] = 0; mag_err[0] = mag_err[1] = mag_err[2] = 0;
} }
@ -568,6 +570,9 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
GPSVelocityGet(&gpsVelData); GPSVelocityGet(&gpsVelData);
HomeLocationGet(&home); HomeLocationGet(&home);
// Discard mag if it has NAN (normally from bad calibration)
mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z);
// Have a minimum requirement for gps usage // Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);