AP_NavEKF3: origin handling fixes from Francisco

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -604,7 +604,11 @@ void NavEKF3_core::readGpsData()
             // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
             if (validOrigin) {
                 gpsDataNew.pos = EKF_origin.get_distance_NE(gpsloc);
-                gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
+                if ((frontend->_originHgtMode & (1<<2)) == 0) {
+                    gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
+                } else {
+                    gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
+                }
                 storedGPS.push(gpsDataNew);
                 // declare GPS available for use
                 gpsNotAvailable = false;

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -219,14 +219,15 @@ bool NavEKF3_core::resetHeightDatum(void)
             // if we don't have GPS lock then we shouldn't be doing a
             // resetHeightDatum, but if we do then the best option is
             // to maintain the old error
-            ekfGpsRefHgt += (int32_t)(100.0f * oldHgt);
+            EKF_origin.alt += (int32_t)(100.0f * oldHgt);
         } else {
             // if we have a good GPS lock then reset to the GPS
             // altitude. This ensures the reported AMSL alt from
             // getLLH() is equal to GPS altitude, while also ensuring
             // that the relative alt is zero
-            ekfGpsRefHgt = AP::gps().location().alt*0.01;
+            EKF_origin.alt = AP::gps().location().alt;
         }
+        ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
     }
 
     // set the terrain state to zero (on ground). The adjustment for