From cc2589777f511696b4dda33cd1a49df0467bc384 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Aug 2022 15:25:40 +1000 Subject: [PATCH] AP_Periph: pass correct WGS84 height on GPS peripherals --- Tools/AP_Periph/can.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index a9eb00e0f5..44d9df9f98 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1859,8 +1859,12 @@ void AP_Periph_FW::can_gps_update(void) } pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL; pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; - pkt.height_ellipsoid_mm = loc.alt * 10; pkt.height_msl_mm = loc.alt * 10; + pkt.height_ellipsoid_mm = loc.alt * 10; + float undulation; + if (gps.get_undulation(undulation)) { + pkt.height_ellipsoid_mm -= undulation*1000; + } for (uint8_t i=0; i<3; i++) { // the canard dsdl compiler doesn't understand float16 pkt.ned_velocity[i] = vel[i];