aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: cope better with large GPS yaw changes

Browse Source 
this should cope better with bungee launches when using only GPS for
yaw.
This commit is contained in:
Andrew Tridgell 2013-06-25 12:43:53 +10:00
parent 0d3faeda90
commit 7d42a0562d
2 changed files with 33 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -256,15 +256,6 @@ AP_AHRS_DCM::yaw_error_compass(void)
return rb % _mag_earth;
}
// produce a yaw error value using the GPS. The returned value is proportional
// to sin() of the current heading error in earth frame
float
AP_AHRS_DCM::yaw_error_gps(void)
{
return sinf(ToRad(_gps->ground_course * 0.01f) - yaw);
}
// the _P_gain raises the gain of the PI controller
// when we are spinning fast. See the fastRotations
// paper from Bill.
@ -330,6 +321,9 @@ AP_AHRS_DCM::drift_correction_yaw(void)
float yaw_deltat;
if (use_compass()) {
/*
we are using compass for yaw
*/
if (_compass->last_update != _compass_last_update) {
yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f;
_compass_last_update = _compass->last_update;
@ -346,17 +340,43 @@ AP_AHRS_DCM::drift_correction_yaw(void)
yaw_error = yaw_error_compass();
}
} else if (_flags.fly_forward && have_gps()) {
/*
we are using GPS for yaw
*/
if (_gps->last_fix_time != _gps_last_update &&
_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps->last_fix_time;
if (!_flags.have_initial_yaw) {
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course*0.01f));
new_value = true;
float gps_course_rad = ToRad(_gps->ground_course * 0.01f);
float yaw_error_rad = gps_course_rad - yaw;
yaw_error = sinf(yaw_error_rad);
/* reset yaw to match GPS heading under any of the
following 3 conditions:
1) if we have reached GPS_SPEED_MIN and have never had
yaw information before
2) if the last time we got yaw information from the GPS
is more than 20 seconds ago, which means we may have
suffered from considerable gyro drift
3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
and our yaw error is over 60 degrees, which means very
poor yaw. This can happen on bungee launch when the
operator pulls back the plane rapidly enough then on
release the GPS heading changes very rapidly
*/
if (!_flags.have_initial_yaw ||
yaw_deltat > 20 ||
(_gps->ground_speed >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
// reset DCM matrix based on current yaw
_dcm_matrix.from_euler(roll, pitch, gps_course_rad);
_omega_yaw_P.zero();
_flags.have_initial_yaw = true;
yaw_error = 0;
}
new_value = true;
yaw_error = yaw_error_gps();
}
}

1
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -74,7 +74,6 @@ private:
void drift_correction(float deltat);
void drift_correction_yaw(void);
float yaw_error_compass();
float yaw_error_gps();
void euler_angles(void);
void estimate_wind(Vector3f &velocity);
bool have_gps(void);