EKF: Add feature to use the mag to only update the mag states

EKF/control.cpp

@@ -977,6 +977,18 @@ void Ekf::controlMagFusion()
 				_control_status.flags.mag_hdg = true;
 			}
 
+			// perform switch-over from only updating the mag states to updating all states
+			if (!_control_status.flags.update_mag_states_only && _control_status_prev.flags.update_mag_states_only) {
+				// reset the mag field covariances
+				zeroRows(P, 16, 21);
+				zeroCols(P, 16, 21);
+
+				// re-instate the last used variances
+				for (uint8_t index = 0; index <= 5; index ++) {
+					P[index+16][index+16] = _saved_mag_variance[index];
+				}
+			}
+
 		} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) {
 			// always use heading fusion
 			_control_status.flags.mag_hdg = true;