Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 02:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

DCM: don't reset _have_initial_yaw for GPS heading unless very slow

Browse Source 
wait till we reach 1m/s before we reset _have_initial_yaw. This
prevents us continually resetting the DCM matrix if our ground speed
is close to 3m/s.
This commit is contained in:
Andrew Tridgell 2012-02-25 11:30:59 +11:00
parent 42528e9c33
commit 3cf0eebac8
1 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_DCM/AP_DCM.cpp
View File

@ -32,9 +32,13 @@
//#define Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
//#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
#define SPEEDFILT 300 // centimeters/second
#define ADC_CONSTRAINT 900
// this is the speed in cm/s above which we first get a yaw lock with
// the GPS
#define GPS_SPEED_MIN 300
// this is the speed in cm/s at which we stop using drift correction
// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN
#define GPS_SPEED_RESET 100
void
AP_DCM::set_compass(Compass *compass)
@ -409,8 +413,7 @@ AP_DCM::drift_correction(void)
} else if (_gps && _gps->status() == GPS::GPS_OK) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) {
if (_gps->ground_speed >= GPS_SPEED_MIN) {
if (_have_initial_yaw) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
@ -425,12 +428,17 @@ AP_DCM::drift_correction(void)
_have_initial_yaw = true;
error_course = 0;
}
} else if (_gps->ground_speed >= GPS_SPEED_RESET) {
// we are not going fast enough to use GPS for
// course correction, but we won't reset
// _have_initial_yaw yet, instead we just let
// the gyro handle yaw
error_course = 0;
} else {
// we are moving very slowly. Stop doing
// course correction with the GPS, and instead
// grab the next GPS course value once we
// start moving again.
// we are moving very slowly. Reset
// _have_initial_yaw and adjust our heading
// rapidly next time we get a good GPS ground
// speed
error_course = 0;
_have_initial_yaw = false;
}