Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: use ground_course in preference to ground_course_cd

Browse Source
This commit is contained in:
Peter Barker 2021-01-22 09:19:38 +11:00 committed by Andrew Tridgell
parent 2e2ec79992
commit 358069df24
2 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -246,7 +246,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
// Generate estimate of ground speed vector using GPS // Generate estimate of ground speed vector using GPS
if (gotGPS) { if (gotGPS) {
const float cog = radians(AP::gps().ground_course_cd()*0.01f); const float cog = radians(AP::gps().ground_course());
gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed(); gndVelGPS = Vector2f(cosf(cog), sinf(cog)) * AP::gps().ground_speed();
} }
// If both ADS and GPS data is available, apply a complementary filter // If both ADS and GPS data is available, apply a complementary filter

4
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -528,7 +528,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f; yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
_gps_last_update = _gps.last_fix_time_ms(); _gps_last_update = _gps.last_fix_time_ms();
new_value = true; new_value = true;
const float gps_course_rad = ToRad(_gps.ground_course_cd() * 0.01f); const float gps_course_rad = ToRad(_gps.ground_course());
const float yaw_error_rad = wrap_PI(gps_course_rad - yaw); const float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
yaw_error = sinf(yaw_error_rad); yaw_error = sinf(yaw_error_rad);
@ -1041,7 +1041,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const
if (_flags.fly_forward && _have_position) { if (_flags.fly_forward && _have_position) {
float gps_delay_sec = 0; float gps_delay_sec = 0;
_gps.get_lag(gps_delay_sec); _gps.get_lag(gps_delay_sec);
loc.offset_bearing(_gps.ground_course_cd() * 0.01f, _gps.ground_speed() * gps_delay_sec); loc.offset_bearing(_gps.ground_course(), _gps.ground_speed() * gps_delay_sec);
} }
return _have_position; return _have_position;
} }