GPS: implement the GPS_DRV_OPTION for ellisoid height in mode drivers (4.4 backport)

libraries/AP_GPS/AP_GPS.cpp

@@ -314,7 +314,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Param: _DRV_OPTIONS
     // @DisplayName: driver options
     // @Description: Additional backend specific options
-    // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL for uBlox driver
+    // @Bitmask: 0:Use UART2 for moving baseline on ublox,1:Use base station for GPS yaw on SBF,2:Use baudrate 115200,3:Use dedicated CAN port b/w GPSes for moving baseline,4:Use ellipsoid height instead of AMSL
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 

libraries/AP_GPS/AP_GPS_ERB.cpp

@@ -150,9 +150,9 @@ AP_GPS_ERB::_parse_gps(void)
         _last_pos_time        = _buffer.pos.time;
         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;
+        set_alt_amsl_cm(state, _buffer.pos.altitude_msl * 100);
         state.status          = next_fix;
         _new_position = true;
         state.horizontal_accuracy = _buffer.pos.horizontal_accuracy * 1.0e-3f;

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -329,7 +329,7 @@ bool AP_GPS_NMEA::_term_complete()
             case _GPS_SENTENCE_GGA:
                 _last_GGA_ms = now;
                 if (_last_KSXT_pos_ms == 0 && _last_AGRICA_ms == 0) {
-                    state.location.alt  = _new_altitude;
+                    set_alt_amsl_cm(state, _new_altitude);
                     state.location.lat  = _new_latitude;
                     state.location.lng  = _new_longitude;
                 }
@@ -426,7 +426,7 @@ bool AP_GPS_NMEA::_term_complete()
                 }
                 state.location.lat     = _ksxt.fields[2]*1.0e7;
                 state.location.lng     = _ksxt.fields[1]*1.0e7;
-                state.location.alt     = _ksxt.fields[3]*1.0e2;
+                set_alt_amsl_cm(state, _ksxt.fields[3]*1.0e2);
                 _last_KSXT_pos_ms = now;
                 if (_ksxt.fields[9] >= 1) {
                     // we have 3D fix
@@ -458,8 +458,9 @@ 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.location.alt = ag.alt*1.0e2;
                 state.undulation   = -ag.undulation;
+                state.have_undulation = true;
+                set_alt_amsl_cm(state, ag.alt*1.0e2);
                 state.velocity = ag.vel_NED;
                 velocity_to_speed_course(state);
                 state.speed_accuracy = ag.vel_stddev.length();
@@ -469,7 +470,6 @@ bool AP_GPS_NMEA::_term_complete()
                 state.have_speed_accuracy = true;
                 state.have_horizontal_accuracy = true;
                 state.have_vertical_accuracy = true;
-                state.have_undulation = true;
                 check_new_itow(ag.itow, _sentence_length);
                 break;
             }

libraries/AP_GPS/AP_GPS_NOVA.cpp

@@ -205,9 +205,9 @@ 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;
+        set_alt_amsl_cm(state, bestposu.hgt * 100);
 
         state.num_sats = bestposu.svsused;
 

libraries/AP_GPS/AP_GPS_SBF.cpp

@@ -415,9 +415,9 @@ AP_GPS_SBF::process_message(void)
         if (temp.Latitude > -200000) {
             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;
+            set_alt_amsl_cm(state, ((float)temp.Height - temp.Undulation) * 1e2f);
         }
 
         if (temp.NrSV != 255) {

libraries/AP_GPS/AP_GPS_SIRF.cpp

@@ -179,9 +179,12 @@ 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);
+        const int32_t alt_amsl = swap_int32(_buffer.nav.altitude_msl);
+        const int32_t alt_ellipsoid = swap_int32(_buffer.nav.altitude_ellipsoid);
         state.have_undulation = true;
-        state.undulation = (state.location.alt - swap_int32(_buffer.nav.altitude_ellipsoid))*0.01;
+        state.undulation = (alt_amsl - alt_ellipsoid)*0.01;
+        set_alt_amsl_cm(state, alt_amsl);
+        state.location.alt      = swap_int32(_buffer.nav.altitude_msl);
         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;

libraries/AP_GPS/AP_GPS_UAVCAN.cpp

@@ -436,10 +436,11 @@ void AP_GPS_UAVCAN::handle_fix2_msg(const Fix2Cb &cb)
         Location loc = { };
         loc.lat = cb.msg->latitude_deg_1e8 / 10;
         loc.lng = cb.msg->longitude_deg_1e8 / 10;
-        loc.alt = cb.msg->height_msl_mm / 10;
+        const int32_t alt_amsl_cm = cb.msg->height_msl_mm / 10;
         interim_state.have_undulation = true;
         interim_state.undulation = (cb.msg->height_msl_mm - cb.msg->height_ellipsoid_mm) * 0.001;
         interim_state.location = loc;
+        set_alt_amsl_cm(interim_state, alt_amsl_cm);
 
         handle_velocity(cb.msg->ned_velocity[0], cb.msg->ned_velocity[1], cb.msg->ned_velocity[2]);
 

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1368,13 +1368,9 @@ AP_GPS_UBLOX::_parse_gps(void)
         _last_pos_time        = _buffer.posllh.itow;
         state.location.lng    = _buffer.posllh.longitude;
         state.location.lat    = _buffer.posllh.latitude;
-        if (option_set(AP_GPS::HeightEllipsoid)) {
-            state.location.alt    = _buffer.posllh.altitude_ellipsoid / 10;
-        } 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;
+        set_alt_amsl_cm(state, _buffer.posllh.altitude_msl / 10);
 
         state.status          = next_fix;
         _new_position = true;
@@ -1532,13 +1528,9 @@ AP_GPS_UBLOX::_parse_gps(void)
         _last_pos_time        = _buffer.pvt.itow;
         state.location.lng    = _buffer.pvt.lon;
         state.location.lat    = _buffer.pvt.lat;
-        if (option_set(AP_GPS::HeightEllipsoid)) {
-            state.location.alt    = _buffer.pvt.h_ellipsoid / 10;
-        } 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;
+        set_alt_amsl_cm(state, _buffer.pvt.h_msl / 10);
         switch (_buffer.pvt.fix_type)
         {
             case 0:

libraries/AP_GPS/GPS_Backend.cpp

@@ -468,6 +468,21 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state,
 }
 #endif // GPS_MOVING_BASELINE
 
+/*
+  set altitude in location structure, honouring the driver option for
+  MSL vs ellipsoid height
+ */
+void AP_GPS_Backend::set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm)
+{
+    if (option_set(AP_GPS::HeightEllipsoid) && _state.have_undulation) {
+        // user has asked ArduPilot to use ellipsoid height in the
+        // canonical height for mission and navigation
+        _state.location.alt = alt_amsl_cm - _state.undulation*100;
+    } else {
+        _state.location.alt = alt_amsl_cm;
+    }
+}
+
 #if AP_GPS_DEBUG_LOGGING_ENABLED
 
 /*

libraries/AP_GPS/GPS_Backend.h

@@ -157,6 +157,9 @@ class AP_GPS_Backend
     void log_data(const uint8_t *data, uint16_t length);
 #endif
 
+    // set alt in location, honouring GPS driver option for ellipsoid height
+    void set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm);
+
 private:
     // itow from previous message
     uint64_t _pseudo_itow;