aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 Kp_YAW 0.8 // Yaw Drift Correction Porportional Gain
//#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain //#define Ki_YAW 0.00004 // Yaw Drift CorrectionIntegrator Gain
#define SPEEDFILT 300 // centimeters/second // this is the speed in cm/s above which we first get a yaw lock with
#define ADC_CONSTRAINT 900 // 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 void
AP_DCM::set_compass(Compass *compass) AP_DCM::set_compass(Compass *compass)
@ -409,8 +413,7 @@ AP_DCM::drift_correction(void)
} else if (_gps && _gps->status() == GPS::GPS_OK) { } else if (_gps && _gps->status() == GPS::GPS_OK) {
// Use GPS Ground course to correct yaw gyro drift // 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) { if (_have_initial_yaw) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0)); float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(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; _have_initial_yaw = true;
error_course = 0; 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 { } else {
// we are moving very slowly. Stop doing // we are moving very slowly. Reset
// course correction with the GPS, and instead // _have_initial_yaw and adjust our heading
// grab the next GPS course value once we // rapidly next time we get a good GPS ground
// start moving again. // speed
error_course = 0; error_course = 0;
_have_initial_yaw = false; _have_initial_yaw = false;
} }