From 378dff5568b3636e94236f9f08cf093d9d494694 Mon Sep 17 00:00:00 2001
From: Joshua Henderson <hendjoshsr71@gmail.com>
Date: Thu, 28 Jul 2022 04:42:01 -0400
Subject: [PATCH] AP_GPS: added get_undulation support

---
 libraries/AP_GPS/AP_GPS.cpp       | 40 ++++++++++++++++++++++++++++---
 libraries/AP_GPS/AP_GPS.h         | 12 ++++++++++
 libraries/AP_GPS/AP_GPS_ERB.cpp   |  2 ++
 libraries/AP_GPS/AP_GPS_NOVA.cpp  |  2 ++
 libraries/AP_GPS/AP_GPS_SBF.cpp   |  2 ++
 libraries/AP_GPS/AP_GPS_SIRF.cpp  |  2 ++
 libraries/AP_GPS/AP_GPS_UBLOX.cpp |  5 ++++
 libraries/AP_GPS/LogStructure.h   |  4 +++-
 8 files changed, 65 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 95f7599b0d..f3fb2c41e1 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -1353,6 +1353,11 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
     float hacc = 0.0f;
     float vacc = 0.0f;
     float sacc = 0.0f;
+    float undulation = 0.0;
+    int32_t height_elipsoid_mm = 0;
+    if (get_undulation(0, undulation)) {
+        height_elipsoid_mm = loc.alt*10 - undulation*1000;
+    }
     horizontal_accuracy(0, hacc);
     vertical_accuracy(0, vacc);
     speed_accuracy(0, sacc);
@@ -1368,7 +1373,7 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan)
         ground_speed(0)*100,  // cm/s
         ground_course(0)*100, // 1/100 degrees,
         num_sats(0),
-        0,                    // TODO: Elipsoid height in mm
+        height_elipsoid_mm,   // Elipsoid height in mm
         hacc * 1000,          // one-sigma standard deviation in mm
         vacc * 1000,          // one-sigma standard deviation in mm
         sacc * 1000,          // one-sigma standard deviation in mm/s
@@ -1388,6 +1393,11 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
     float hacc = 0.0f;
     float vacc = 0.0f;
     float sacc = 0.0f;
+    float undulation = 0.0;
+    float height_elipsoid_mm = 0;
+    if (get_undulation(0, undulation)) {
+        height_elipsoid_mm = loc.alt*10 - undulation*1000;
+    }
     horizontal_accuracy(1, hacc);
     vertical_accuracy(1, vacc);
     speed_accuracy(1, sacc);
@@ -1406,7 +1416,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan)
         state[1].rtk_num_sats,
         state[1].rtk_age_ms,
         gps_yaw_cdeg(1),
-        0,                    // TODO: Elipsoid height in mm
+        height_elipsoid_mm,   // Elipsoid height in mm
         hacc * 1000,          // one-sigma standard deviation in mm
         vacc * 1000,          // one-sigma standard deviation in mm
         sacc * 1000,          // one-sigma standard deviation in mm/s
@@ -1855,6 +1865,16 @@ void AP_GPS::calc_blended_state(void)
     timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0;
     timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0;
 
+    if (state[0].have_undulation) {
+        state[GPS_BLENDED_INSTANCE].have_undulation = true;
+        state[GPS_BLENDED_INSTANCE].undulation = state[0].undulation;
+    } else if (state[1].have_undulation) {
+        state[GPS_BLENDED_INSTANCE].have_undulation = true;
+        state[GPS_BLENDED_INSTANCE].undulation = state[1].undulation;
+    } else {
+        state[GPS_BLENDED_INSTANCE].have_undulation = false;
+    }
+
     // combine the states into a blended solution
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         // use the highest status
@@ -2100,6 +2120,17 @@ bool AP_GPS::get_error_codes(uint8_t instance, uint32_t &error_codes) const
     return drivers[instance]->get_error_codes(error_codes);
 }
 
+// get the difference between WGS84 and AMSL. A positive value means
+// the AMSL height is higher than WGS84 ellipsoid height
+bool AP_GPS::get_undulation(uint8_t instance, float &undulation) const
+{
+    if (!state[instance].have_undulation) {
+        return false;
+    }
+    undulation = state[instance].undulation;
+    return true;
+}
+
 // Logging support:
 // Write an GPS packet
 void AP_GPS::Write_GPS(uint8_t i)
@@ -2133,9 +2164,11 @@ void AP_GPS::Write_GPS(uint8_t i)
 
     /* write auxiliary accuracy information as well */
     float hacc = 0, vacc = 0, sacc = 0;
+    float undulation = 0;
     horizontal_accuracy(i, hacc);
     vertical_accuracy(i, vacc);
     speed_accuracy(i, sacc);
+    get_undulation(i, undulation);
     struct log_GPA pkt2{
         LOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
         time_us       : time_us,
@@ -2147,7 +2180,8 @@ void AP_GPS::Write_GPS(uint8_t i)
         yaw_accuracy  : yaw_accuracy_deg,
         have_vv       : (uint8_t)have_vertical_velocity(i),
         sample_ms     : last_message_time_ms(i),
-        delta_ms      : last_message_delta_time_ms(i)
+        delta_ms      : last_message_delta_time_ms(i),
+        undulation    : undulation,
     };
     AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
 }
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 5b2b5290a8..cc8a13e7e1 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -197,6 +197,8 @@ public:
         bool have_vertical_accuracy;      ///< does GPS give vertical position accuracy? Set to true only once available.
         bool have_gps_yaw;                ///< does GPS give yaw? Set to true only once available.
         bool have_gps_yaw_accuracy;       ///< does the GPS give a heading accuracy estimate? Set to true only once available
+        float undulation;                   //<height that WGS84 is above AMSL at the current location
+        bool have_undulation;               ///<do we have a value for the undulation
         uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
         uint64_t last_corrected_gps_time_us;///< the system time we got the last corrected GPS timestamp, microseconds
         bool corrected_timestamp_updated;  ///< true if the corrected timestamp has been updated
@@ -297,6 +299,16 @@ public:
         return location(primary_instance);
     }
 
+    // get the difference between WGS84 and AMSL. A positive value means
+    // the AMSL height is higher than WGS84 ellipsoid height
+    bool get_undulation(uint8_t instance, float &undulation) const;
+
+    // get the difference between WGS84 and AMSL. A positive value means
+    // the AMSL height is higher than WGS84 ellipsoid height
+    bool get_undulation(float &undulation) const {
+        return get_undulation(primary_instance, undulation);
+    }
+
     // report speed accuracy
     bool speed_accuracy(uint8_t instance, float &sacc) const;
     bool speed_accuracy(float &sacc) const {
diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp
index 6185c325e3..b125154d10 100644
--- a/libraries/AP_GPS/AP_GPS_ERB.cpp
+++ b/libraries/AP_GPS/AP_GPS_ERB.cpp
@@ -148,6 +148,8 @@ 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.location.alt    = (int32_t)(_buffer.pos.altitude_msl * 100);
+        state.have_undulation = true;
+        state.undulation = _buffer.pos.altitude_msl - _buffer.pos.altitude_ellipsoid;
         state.status          = next_fix;
         _new_position = true;
         state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f;
diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp
index 60b17df383..b70c15af21 100644
--- a/libraries/AP_GPS/AP_GPS_NOVA.cpp
+++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp
@@ -205,6 +205,8 @@ AP_GPS_NOVA::process_message(void)
         state.location.lat = (int32_t) (bestposu.lat * (double)1e7);
         state.location.lng = (int32_t) (bestposu.lng * (double)1e7);
         state.location.alt = (int32_t) (bestposu.hgt * 100);
+        state.have_undulation = true;
+        state.undulation = bestposu.undulation;
 
         state.num_sats = bestposu.svsused;
 
diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp
index 60d6a80c92..5abd60b288 100644
--- a/libraries/AP_GPS/AP_GPS_SBF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SBF.cpp
@@ -416,6 +416,8 @@ AP_GPS_SBF::process_message(void)
             state.location.lat = (int32_t)(temp.Latitude * RAD_TO_DEG_DOUBLE * (double)1e7);
             state.location.lng = (int32_t)(temp.Longitude * RAD_TO_DEG_DOUBLE * (double)1e7);
             state.location.alt = (int32_t)(((float)temp.Height - temp.Undulation) * 1e2f);
+            state.have_undulation = true;
+            state.undulation = temp.Undulation;
         }
 
         if (temp.NrSV != 255) {
diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp
index 18f42155c7..d758003c3f 100644
--- a/libraries/AP_GPS/AP_GPS_SIRF.cpp
+++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp
@@ -180,6 +180,8 @@ AP_GPS_SIRF::_parse_gps(void)
         state.location.lat      = swap_int32(_buffer.nav.latitude);
         state.location.lng      = swap_int32(_buffer.nav.longitude);
         state.location.alt      = swap_int32(_buffer.nav.altitude_msl);
+        state.have_undulation = true;
+        state.undulation = (state.location.alt - swap_int32(_buffer.nav.altitude_ellipsoid))*0.01;
         state.ground_speed      = swap_int32(_buffer.nav.ground_speed)*0.01f;
         state.ground_course     = wrap_360(swap_int16(_buffer.nav.ground_course)*0.01f);
         state.num_sats          = _buffer.nav.satellites;
diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
index 10791d651c..831fbaba90 100644
--- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp
+++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp
@@ -1292,6 +1292,9 @@ AP_GPS_UBLOX::_parse_gps(void)
         } else {
             state.location.alt    = _buffer.posllh.altitude_msl / 10;
         }
+        state.have_undulation = true;
+        state.undulation = (_buffer.posllh.altitude_msl - _buffer.posllh.altitude_ellipsoid) * 0.001;
+
         state.status          = next_fix;
         _new_position = true;
         state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
@@ -1449,6 +1452,8 @@ AP_GPS_UBLOX::_parse_gps(void)
         } else {
             state.location.alt    = _buffer.pvt.h_msl / 10;
         }
+        state.have_undulation = true;
+        state.undulation = (_buffer.pvt.h_msl - _buffer.pvt.h_ellipsoid) * 0.001;
         switch (_buffer.pvt.fix_type)
         {
             case 0:
diff --git a/libraries/AP_GPS/LogStructure.h b/libraries/AP_GPS/LogStructure.h
index cfe711ae42..e34bed923f 100644
--- a/libraries/AP_GPS/LogStructure.h
+++ b/libraries/AP_GPS/LogStructure.h
@@ -62,6 +62,7 @@ struct PACKED log_GPS {
 // @Field: VV: true if vertical velocity is available
 // @Field: SMS: time since system startup this sample was taken
 // @Field: Delta: system time delta between the last two reported positions
+// @Field: Und: Undulation
 struct PACKED log_GPA {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -74,6 +75,7 @@ struct PACKED log_GPA {
     uint8_t  have_vv;
     uint32_t sample_ms;
     uint16_t delta_ms;
+    float undulation;
 };
 
 /*
@@ -200,7 +202,7 @@ struct PACKED log_GPS_RAWS {
     { LOG_GPS_MSG, sizeof(log_GPS), \
       "GPS",  "QBBIHBcLLeffffB", "TimeUS,I,Status,GMS,GWk,NSats,HDop,Lat,Lng,Alt,Spd,GCrs,VZ,Yaw,U", "s#---SmDUmnhnh-", "F----0BGGB000--" , true }, \
     { LOG_GPA_MSG,  sizeof(log_GPA), \
-      "GPA",  "QBCCCCfBIH", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta", "s#mmmnd-ss", "F-BBBB0-CC" , true }, \
+      "GPA",  "QBCCCCfBIHf", "TimeUS,I,VDop,HAcc,VAcc,SAcc,YAcc,VV,SMS,Delta,Und", "s#mmmnd-ssm", "F-BBBB0-CC0" , true }, \
     { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
       "UBX1", "QBHBBHI",  "TimeUS,Instance,noisePerMS,jamInd,aPower,agcCnt,config", "s#-----", "F------"  , true }, \
     { LOG_GPS_UBX2_MSG, sizeof(log_Ubx2), \