Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AHRS: cope with copters with no compass 

if a copter doesn't have a compass, we can't use the GPS for gyro
drift correction
This commit is contained in:
Andrew Tridgell 2012-06-22 18:27:22 +10:00
parent 09db935874
commit e4a245179c
1 changed files with 20 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -342,6 +342,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
Vector3f error;
Vector3f velocity;
uint32_t last_correction_time;
bool use_gps = true;
// perform yaw drift correction if we have a new yaw reference
// vector
@ -354,7 +355,20 @@ AP_AHRS_DCM::drift_correction(float deltat)
// we have integrated over
_ra_deltat += deltat;
// check if we have GPS lock
if (_gps == NULL || _gps->status() != GPS::GPS_OK) {
use_gps = false;
}
// a copter (which has _fly_forward false) which doesn't have a
// compass for yaw can't rely on the GPS velocity lining up with
// the earth frame from DCM, so it needs to assume zero velocity
// in the drift correction
if (!_fly_forward && !(_compass && _compass->use_for_yaw())) {
use_gps = false;
}
if (use_gps == false) {
// no GPS, or no lock. We assume zero velocity. This at
// least means we can cope with gyro drift while sitting
// on a bench with no GPS lock
@ -371,12 +385,14 @@ AP_AHRS_DCM::drift_correction(float deltat)
return;
}
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), 0);
last_correction_time = _gps->last_fix_time;
}
// even without GPS lock we can correct for the vertical acceleration
if (_barometer != NULL) {
// Z velocity is down
velocity.z = - _barometer->get_climb_rate();
}
last_correction_time = _gps->last_fix_time;
}
// see if this is our first time through - in which case we
// just setup the start times and return