AP_GPS: added GPS_DRV_OPTIONS bit for ellipsoid height

libraries/AP_GPS/AP_GPS.cpp

@@ -309,7 +309,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
+    // @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
     // @User: Advanced
     AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 #endif

libraries/AP_GPS/AP_GPS.h

@@ -593,6 +593,7 @@ protected:
         SBF_UseBaseForYaw = (1U << 1U),
         UBX_Use115200     = (1U << 2U),
         UAVCAN_MBUseDedicatedBus  = (1 << 3U),
+        HeightEllipsoid   = (1U << 4),
     };
 
     // check if an option is set

libraries/AP_GPS/AP_GPS_UBLOX.cpp

@@ -1284,7 +1284,11 @@ AP_GPS_UBLOX::_parse_gps(void)
         _last_pos_time        = _buffer.posllh.itow;
         state.location.lng    = _buffer.posllh.longitude;
         state.location.lat    = _buffer.posllh.latitude;
-        state.location.alt    = _buffer.posllh.altitude_msl / 10;
+        if (option_set(AP_GPS::HeightEllipsoid)) {
+            state.location.alt    = _buffer.posllh.altitude_ellipsoid / 10;
+        } else {
+            state.location.alt    = _buffer.posllh.altitude_msl / 10;
+        }
         state.status          = next_fix;
         _new_position = true;
         state.horizontal_accuracy = _buffer.posllh.horizontal_accuracy*1.0e-3f;
@@ -1437,8 +1441,12 @@ AP_GPS_UBLOX::_parse_gps(void)
         _last_pos_time        = _buffer.pvt.itow;
         state.location.lng    = _buffer.pvt.lon;
         state.location.lat    = _buffer.pvt.lat;
-        state.location.alt    = _buffer.pvt.h_msl / 10;
-        switch (_buffer.pvt.fix_type) 
+        if (option_set(AP_GPS::HeightEllipsoid)) {
+            state.location.alt    = _buffer.pvt.h_ellipsoid / 10;
+        } else {
+            state.location.alt    = _buffer.pvt.h_msl / 10;
+        }
+        switch (_buffer.pvt.fix_type)
         {
             case 0:
                 state.status = AP_GPS::NO_FIX;

libraries/AP_GPS/AP_GPS_UBLOX.h

@@ -346,7 +346,7 @@ private:
         uint8_t flags2; 
         uint8_t num_sv; 
         int32_t lon, lat; 
-        int32_t height, h_msl; 
+        int32_t h_ellipsoid, h_msl;
         uint32_t h_acc, v_acc; 
         int32_t velN, velE, velD, gspeed; 
         int32_t head_mot;