AP_NavEKF2: remove code that prevents attitude updates after mag reset

This code assumes a vehicle is close to level and will not work for all vehicle types. It will be replaced by a different method.

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -43,8 +43,6 @@ void NavEKF2_core::controlMagYawReset()
 
         if (hgtCheckPassed || toiletBowling) {
             firstMagYawInit = true;
-            // reset the timer used to prevent magnetometer fusion from affecting attitude until initial field learning is complete
-            magFuseTiltInhibit_ms =  imuSampleTime_ms;
             // Update the yaw  angle and earth field states using the magnetic field measurements
             Quaternion tempQuat;
             Vector3f eulerAngles;
@@ -64,14 +62,6 @@ void NavEKF2_core::controlMagYawReset()
         firstMagYawInit = true;
     }
 
-    // inhibit the 3-axis mag fusion from modifying the tilt states for the first few seconds after a mag field reset
-    // to allow the mag states to converge and prevent disturbances in roll and pitch.
-    if (imuSampleTime_ms - magFuseTiltInhibit_ms < 5000) {
-        magFuseTiltInhibit = true;
-    } else {
-        magFuseTiltInhibit = false;
-    }
-
 }
 
 // this function is used to do a forced re-alignment of the yaw angle to align with the horizontal velocity
@@ -627,12 +617,6 @@ void NavEKF2_core::FuseMagnetometer()
             statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
         }
 
-        // Inhibit corrections to tilt if requested. This enables mag states to settle after a reset without causing sudden changes in roll and pitch
-        if (magFuseTiltInhibit) {
-            stateStruct.angErr.x = 0.0f;
-            stateStruct.angErr.y = 0.0f;
-        }
-
         // the first 3 states represent the angular misalignment vector. This is
         // is used to correct the estimated quaternion on the current time step
         stateStruct.quat.rotate(stateStruct.angErr);

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -240,7 +240,6 @@ void NavEKF2_core::InitialiseVariables()
     optFlowFusionDelayed = false;
     airSpdFusionDelayed = false;
     sideSlipFusionDelayed = false;
-    magFuseTiltInhibit = false;
     posResetNE.zero();
     velResetNE.zero();
     hgtInnovFiltState = 0.0f;

libraries/AP_NavEKF2/AP_NavEKF2_core.h

@@ -758,8 +758,6 @@ private:
     bool optFlowFusionDelayed;      // true when the optical flow fusion has been delayed
     bool airSpdFusionDelayed;       // true when the air speed fusion has been delayed
     bool sideSlipFusionDelayed;     // true when the sideslip fusion has been delayed
-    bool magFuseTiltInhibit;        // true when the 3-axis magnetoemter fusion is prevented from changing tilt angle
-    uint32_t magFuseTiltInhibit_ms; // time in msec that the condition indicated by magFuseTiltInhibit was commenced
     Vector3f lastMagOffsets;        // Last magnetometer offsets from COMPASS_ parameters. Used to detect parameter changes.
     bool lastMagOffsetsValid;       // True when lastMagOffsets has been initialized
     Vector2f posResetNE;            // Change in North/East position due to last in-flight reset in metres. Returned by getLastPosNorthEastReset