AP_NavEKF3: Fix roundoff, and missing offset handling of getLLH

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -313,10 +313,13 @@ bool NavEKF3_core::getHAGL(float &HAGL) const
 bool NavEKF3_core::getLLH(struct Location &loc) const
 {
     const AP_GPS &gps = AP::gps();
+    Location origin;
+    float posD;
 
-    if(validOrigin) {
+
+    if(getPosD(posD) && getOriginLLH(origin)) {
         // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
-        loc.alt =  100 * (int32_t)(ekfGpsRefHgt - (double)outputDataNew.position.z);
+        loc.alt = origin.alt - posD*100;
         loc.flags.relative_alt = 0;
         loc.flags.terrain_alt = 0;