rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

AHRS: Only do EKF correction with mag when new data available

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1651 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-16 02:58:56 +00:00 committed by peabody124
parent 19e80da928
commit ff1b303083
3 changed files with 20 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
flight/AHRS/ahrs.c
View File

@ -78,6 +78,7 @@ volatile enum algorithms ahrs_algorithm;
*/
struct mag_sensor {
uint8_t id[4];
uint8_t updated;
struct {
int16_t axis[3];
} raw;
@ -296,6 +297,7 @@ int main()
// Get magnetic readings
if (PIOS_HMC5843_NewDataAvailable()) {
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
mag_data.updated = 1;
}
#endif
// Delay for valid data
@ -358,12 +360,21 @@ int main()
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar(0.004);
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
if(gps_data.updated) {
//TOOD: add check for altitude updates
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
gps_data.updated = 0;
} else {
GpsBaroCorrection(gps_data.NED,vel,altitude_data.altitude);
}
gps_data.updated = false;
mag_data.updated = 0;
}
else if(gps_data.quality != -1)
else if(gps_data.quality != -1 && mag_data.updated == 1) {
MagCorrection(mag); // only trust mags if outdoors
else {
mag_data.updated = 0;
} else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;

1
flight/AHRS/inc/insgps.h
View File

@ -61,6 +61,7 @@ void INSSetMagVar(float scaled_mag_var[3]);
void MagCorrection(float mag_data[3]);
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt);
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);

5
flight/AHRS/insgps.c
View File

@ -183,6 +183,11 @@ void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
INSCorrection(mag_data, zeros, Vel, BaroAlt, MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(zeros, Pos, Vel, BaroAlt, HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
}
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt)
{
INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);