AP_NavEKF3: Fix default behaviour for height origin corrections

Previous default was to apply in-flight height origin changes to local position instead of to reported origin height. This caused problems with copters that took off before getting GPS lock.

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -542,8 +542,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
 
     // @Param: OGN_HGT_MASK
     // @DisplayName: Bitmask control of EKF reference height correction
-    // @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position (default) or to the reported EKF origin height.
+    // @Description: When a height sensor other than GPS is used as the primary height source by the EKF, the position of the zero height datum is defined by that sensor and its frame of reference. If a GPS height measurement is also available, then the height of the WGS-84 height datum used by the EKF can be corrected so that the height returned by the getLLH() function is compensated for primary height sensor drift and change in datum over time. The first two bit positions control when the height datum will be corrected. Correction is performed using a Bayes filter and only operates when GPS quality permits. The third bit position controls where the corrections to the GPS reference datum are applied. Corrections can be applied to the local vertical position or to the reported EKF origin height (default).
-    // @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to origin height
+    // @Bitmask: 0:Correct when using Baro height,1:Correct when using range finder height,2:Apply corrections to local position
     // @User: Advanced
     // @RebootRequired: True
     AP_GROUPINFO("OGN_HGT_MASK", 50, NavEKF3, _originHgtMode, 0),

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -283,8 +283,8 @@ bool NavEKF3_core::getPosD(float &posD) const
 {
     // The EKF always has a height estimate regardless of mode of operation
     // Correct for the IMU offset (EKF calculations are at the IMU)
-    // Correct for
+    // Also correct for changes to the origin height
-    if (frontend->_originHgtMode & (1<<2)) {
+    if ((frontend->_originHgtMode & (1<<2)) == 0) {
         // Any sensor height drift corrections relative to the WGS-84 reference are applied to the origin.
         posD = outputDataNew.position.z + posOffsetNED.z;
     } else {
@@ -381,7 +381,7 @@ bool NavEKF3_core::getOriginLLH(struct Location &loc) const
     if (validOrigin) {
         loc = EKF_origin;
         // report internally corrected reference height if enabled
-        if (frontend->_originHgtMode & (1<<2)) {
+        if ((frontend->_originHgtMode & (1<<2)) == 0) {
             loc.alt = (int32_t)(100.0f * (float)ekfGpsRefHgt);
         }
     }