From 47b14a7d60a8722f962256b560f1e9dabbf7c07b Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Fri, 31 Jan 2025 01:08:27 +0000 Subject: [PATCH] AP_GPS: Fix undulation incorrect convention * And some drivers likely reporting the wrong ellipsoid height in GPS_RAW_INT Signed-off-by: Ryan Friedman --- libraries/AP_GPS/AP_GPS.cpp | 4 ++-- libraries/AP_GPS/AP_GPS_DroneCAN.cpp | 2 +- libraries/AP_GPS/AP_GPS_ERB.cpp | 2 +- libraries/AP_GPS/AP_GPS_NMEA.cpp | 2 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 4 ++-- 6 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 4d6f1e164ccc9..6c2b35290ba1d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1377,7 +1377,7 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) float undulation = 0.0; int32_t height_elipsoid_mm = 0; if (get_undulation(0, undulation)) { - height_elipsoid_mm = loc.alt*10 - undulation*1000; + height_elipsoid_mm = loc.alt*10 + undulation*1000; } horizontal_accuracy(0, hacc); vertical_accuracy(0, vacc); @@ -1418,7 +1418,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) float undulation = 0.0; float height_elipsoid_mm = 0; if (get_undulation(1, undulation)) { - height_elipsoid_mm = loc.alt*10 - undulation*1000; + height_elipsoid_mm = loc.alt*10 + undulation*1000; } horizontal_accuracy(1, hacc); vertical_accuracy(1, vacc); diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index aeec0442289af..5965f2f4c8cf9 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -382,7 +382,7 @@ void AP_GPS_DroneCAN::handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uin loc.lng = msg.longitude_deg_1e8 / 10; const int32_t alt_amsl_cm = msg.height_msl_mm / 10; interim_state.have_undulation = true; - interim_state.undulation = (msg.height_msl_mm - msg.height_ellipsoid_mm) * 0.001; + interim_state.undulation = (msg.height_ellipsoid_mm - msg.height_msl_mm) * 0.001; interim_state.location = loc; set_alt_amsl_cm(interim_state, alt_amsl_cm); diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index 1dde8997f2d85..d0dc772d4cc34 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/libraries/AP_GPS/AP_GPS_ERB.cpp @@ -151,7 +151,7 @@ AP_GPS_ERB::_parse_gps(void) state.location.lng = (int32_t)(_buffer.pos.longitude * (double)1e7); state.location.lat = (int32_t)(_buffer.pos.latitude * (double)1e7); state.have_undulation = true; - state.undulation = _buffer.pos.altitude_msl - _buffer.pos.altitude_ellipsoid; + state.undulation = _buffer.pos.altitude_ellipsoid - _buffer.pos.altitude_msl; set_alt_amsl_cm(state, _buffer.pos.altitude_msl * 100); state.status = next_fix; _new_position = true; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 8d8c0db18501a..1bb8844c060c4 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -458,7 +458,7 @@ bool AP_GPS_NMEA::_term_complete() _last_3D_velocity_ms = now; state.location.lat = ag.lat*1.0e7; state.location.lng = ag.lng*1.0e7; - state.undulation = -ag.undulation; + state.undulation = ag.undulation; state.have_undulation = true; set_alt_amsl_cm(state, ag.alt*1.0e2); state.velocity = ag.vel_NED; diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index dd6eb901c807b..d968532ff5eda 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -185,7 +185,7 @@ AP_GPS_SIRF::_parse_gps(void) state.location.lng = int32_t(be32toh(_buffer.nav.longitude)); const int32_t alt_amsl = int32_t(be32toh(_buffer.nav.altitude_msl)); const int32_t alt_ellipsoid = int32_t(be32toh(_buffer.nav.altitude_ellipsoid)); - state.undulation = (alt_amsl - alt_ellipsoid)*0.01; + state.undulation = (alt_ellipsoid - alt_amsl)*0.01; state.have_undulation = true; set_alt_amsl_cm(state, alt_amsl); state.ground_speed = int32_t(be32toh(_buffer.nav.ground_speed))*0.01f; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index f4b665b7cdbb9..fc81b052eb110 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1495,7 +1495,7 @@ AP_GPS_UBLOX::_parse_gps(void) state.location.lng = _buffer.posllh.longitude; state.location.lat = _buffer.posllh.latitude; state.have_undulation = true; - state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001; + state.undulation = (_buffer.posllh.altitude_ellipsoid - _buffer.posllh.altitude_msl) * 0.001; set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10); state.status = next_fix; @@ -1655,7 +1655,7 @@ AP_GPS_UBLOX::_parse_gps(void) state.location.lng = _buffer.pvt.lon; state.location.lat = _buffer.pvt.lat; state.have_undulation = true; - state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001; + state.undulation = (_buffer.pvt.h_ellipsoid - _buffer.pvt.h_msl) * 0.001; set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10); switch (_buffer.pvt.fix_type) {