EKF: Improve detection of indoor flight condition

EKF/control.cpp

@@ -1400,13 +1400,13 @@ void Ekf::controlMagFusion()
 			_control_status.flags.mag_dec = false;
 		}
 
-		// If optical flow is the only aiding source and GPS checks are failing, then assume that we are operating
+		// If optical flow is enabled there is no other aiding active and GPS checks are failing, then assume that we are operating
 		// indoors and the magnetometer is unreliable. Becasue the optical flow sensor is body fixed, absolute yaw
 		// wrt North is not required for navigation and it is safer not to use the magnetometer.
-		if ((_control_status.flags.opt_flow || _mag_use_inhibit)
+		if (((_params.fusion_mode & MASK_USE_OF) || _mag_use_inhibit)
 				&& !_control_status.flags.gps
 				&& !_control_status.flags.ev_pos
-				&& (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6)) {
+				&& ((_time_last_imu - _last_gps_pass_us > (uint64_t)5e6) || !(_params.fusion_mode & MASK_USE_GPS))) {
 			_mag_use_inhibit = true;
 		} else {
 			_mag_use_inhibit = false;

EKF/ekf.h

@@ -326,6 +326,7 @@ private:
 	bool _mag_use_inhibit{false};		///< true when magnetomer use is being inhibited
 	bool _mag_inhibit_yaw_reset_req{false};	///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve.
 	float _last_static_yaw{0.0f};		///< last yaw angle recorded when on ground motion checks were passing (rad)
+	bool _vehicle_at_rest_prev{false};	///< true when the vehicle was at rest the previous time the status was checked
 
 	float P[_k_num_states][_k_num_states] {};	///< state covariance matrix
 

EKF/mag_fusion.cpp

@@ -624,11 +624,15 @@ void Ekf::fuseHeading()
 			// Vehicle is not at rest so fuse a zero innovation and record the
 			// predicted heading to use as an observation when movement ceases.
 			_heading_innov = 0.0f;
-			_last_static_yaw = predicted_hdg;
+			_vehicle_at_rest_prev = false;
 		} else {
 			// Vehicle is at rest so use the last moving prediciton as an observation
 			// to prevent the heading from drifting and to enable yaw gyro bias learning
 			// before takeoff.
+			if (!_vehicle_at_rest_prev) {
+				_last_static_yaw = predicted_hdg;
+				_vehicle_at_rest_prev = true;
+			}
 			_heading_innov = predicted_hdg - _last_static_yaw;
 			R_YAW = 0.01f;
 			innov_gate = 5.0f;