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

AP_GPS: fixed moving baseline yaw calculation

Browse Source 
this reverts #13955 and instead applies the correct fix, which is to
subtract the angle instead of adding.
This commit is contained in:
Andrew Tridgell 2020-04-02 22:14:11 +11:00
parent 3195a7cccd
commit 3fbeae613b
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -1315,8 +1315,8 @@ AP_GPS_UBLOX::_parse_gps(void)
break; break;
case MSG_RELPOSNED: case MSG_RELPOSNED:
{ {
const Vector3f &offset0 = gps._antenna_offset[0].get(); const Vector3f &offset0 = gps._antenna_offset[state.instance^1].get();
const Vector3f &offset1 = gps._antenna_offset[1].get(); const Vector3f &offset1 = gps._antenna_offset[state.instance].get();
// note that we require the yaw to come from a fixed solution, not a float solution // note that we require the yaw to come from a fixed solution, not a float solution
// yaw from a float solution would only be acceptable with a very large separation between // yaw from a float solution would only be acceptable with a very large separation between
// GPS modules // GPS modules
@ -1350,10 +1350,7 @@ AP_GPS_UBLOX::_parse_gps(void)
float rotation_offset_rad; float rotation_offset_rad;
const Vector3f diff = offset1 - offset0; const Vector3f diff = offset1 - offset0;
rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); rotation_offset_rad = Vector2f(diff.x, diff.y).angle();
if (state.instance != 0) { state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 - degrees(rotation_offset_rad));
rotation_offset_rad += M_PI;
}
state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 + degrees(rotation_offset_rad));
state.have_gps_yaw = true; state.have_gps_yaw = true;
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5; state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
state.have_gps_yaw_accuracy = true; state.have_gps_yaw_accuracy = true;
@ -1890,6 +1887,12 @@ void AP_GPS_UBLOX::clear_RTCMV3(void)
// ublox specific healthy checks // ublox specific healthy checks
bool AP_GPS_UBLOX::is_healthy(void) const bool AP_GPS_UBLOX::is_healthy(void) const
{ {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (gps._auto_config == AP_GPS::GPS_AUTO_CONFIG_DISABLE) {
// allow for fake ublox moving baseline
return true;
}
#endif
if ((role == AP_GPS::GPS_ROLE_MB_BASE || if ((role == AP_GPS::GPS_ROLE_MB_BASE ||
role == AP_GPS::GPS_ROLE_MB_ROVER) && role == AP_GPS::GPS_ROLE_MB_ROVER) &&
!supports_F9_config()) { !supports_F9_config()) {