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

AP_GPS: fixed yaw error when one GPS has zero position 

the M_PI correction was only valid for one zero side. Much simpler to
always calculate difference
This commit is contained in:
Andrew Tridgell 2020-01-21 07:50:34 +11:00
parent 141099078c
commit e59f32074d
1 changed files with 2 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1344,14 +1344,8 @@ AP_GPS_UBLOX::_parse_gps(void)
offset_dist > min_separation &&
fabsf(offset_dist - rel_dist) / MIN(offset_dist, rel_dist) < strict_length_error_allowed) {
float rotation_offset_rad;
if (offset0.is_zero()) {
rotation_offset_rad = Vector2f(offset1.x, offset1.y).angle();
} else if (offset1.is_zero()) {
rotation_offset_rad = Vector2f(offset0.x, offset0.y).angle();
} else {
const Vector3f diff = offset0 - offset1;
rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
}
const Vector3f diff = offset0 - offset1;
rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
if (state.instance != 0) {
rotation_offset_rad += M_PI;
}