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

AHRS: In outdoor mode when no GPS for a few seconds start updating with zero

Browse Source 
velocity to help stabilize attitude.  In principle the mag should be fine but
in practice I need this.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2160 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-11-26 15:57:14 +00:00 committed by peabody124
parent b3251e2cfb
commit 75363896fb
1 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
flight/AHRS/ahrs.c
View File

@ -139,25 +139,26 @@ void ins_outdoor_update()
INSCovariancePrediction(1 / (float)EKF_RATE);
sensors = 0;
if (gps_data.updated) {
uint32_t this_gps_time = timer_count();
float gps_delay;
/*
* Detect if greater than certain time since last gps update and if so
* reset EKF to that position since probably drifted too far for safe
* update
*/
uint32_t this_gps_time = timer_count();
float gps_delay;
if (this_gps_time < last_gps_time)
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
else
gps_delay = (this_gps_time - last_gps_time) / timer_rate();
last_gps_time = this_gps_time;
if (gps_data.updated) {
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
vel[2] = 0;
/*
* Detect if greater than certain time since last gps update and if so
* reset EKF to that position since probably drifted too far for safe
* update
*/
if (this_gps_time < last_gps_time)
gps_delay = ((0xFFFF - last_gps_time) - this_gps_time) / timer_rate();
else
gps_delay = (this_gps_time - last_gps_time) / timer_rate();
last_gps_time = this_gps_time;
if(gps_delay > INSGPS_GPS_TIMEOUT)
INSPosVelReset(gps_data.NED,vel); // position stale, reset
else {
@ -176,6 +177,11 @@ void ins_outdoor_update()
baro_offset = baro_offset * 0.99 + (gps_data.NED[2] + altitude_data.altitude) * 0.01;
}
gps_data.updated = false;
} else if (gps_delay > INSGPS_GPS_TIMEOUT) {
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
sensors |= VERT_SENSORS | HORIZ_SENSORS;
}
if(mag_data.updated) {