rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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

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

@ -358,11 +358,12 @@ 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);
// If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
rot_mult(Rbe, home.Be, brot); rot_mult(Rbe, home.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
@ -380,6 +381,7 @@ static int32_t updateAttitudeComplimentary(bool first_run)
mag_err[0] = mag_err[1] = mag_err[2] = 0; mag_err[0] = mag_err[1] = mag_err[2] = 0;
else else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); 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);