From a4fa99c96cd0b64799bfab6940cca9fa9d67dbd1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 27 Aug 2022 19:22:54 +1000 Subject: [PATCH] AP_Periph: fixed undulation in Fix2 pkt --- Tools/AP_Periph/can.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index 12a6d1445a..7442777d97 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1956,6 +1956,10 @@ void AP_Periph_FW::can_gps_update(void) pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL; pkt.height_ellipsoid_mm = loc.alt * 10; pkt.height_msl_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++) { pkt.ned_velocity[i] = vel[i]; }