Update src/modules/ekf2/EKF2.cpp

Co-authored-by: Matthias Grob <maetugr@gmail.com>

.vscode/tasks.json

@@ -170,7 +170,7 @@
             ]
         },
         {
-            "label": "ign gazebo",
+            "label": "gazebo",
             "type": "shell",
             "options": {
                 "cwd": "${workspaceFolder}",
@@ -178,7 +178,7 @@
                     "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gz/models",
                 }
             },
-            "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
+            "command": "gz sim -v 4 -r ${workspaceFolder}/Tools/simulation/gz/worlds/${input:gzWorld}.sdf",
             "isBackground": true,
             "presentation": {
                 "echo": true,
@@ -191,7 +191,7 @@
                 "close": false
             },
             "problemMatcher": [],
-            "dependsOn":["ign gazebo kill"]
+            "dependsOn":["gazebo kill"]
         },
         {
             "label": "gazebo-classic kill",
@@ -211,9 +211,9 @@
             "dependsOn":["px4_sitl_cleanup"]
         },
         {
-            "label": "ign gazebo kill",
+            "label": "gazebo kill",
             "type": "shell",
-            "command": "pkill -9 -f 'ign gazebo' || true",
+            "command": "pkill -9 -f 'gz sim' || true",
             "presentation": {
                 "echo": true,
                 "reveal": "never",

ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480.post

@@ -1,8 +1,6 @@
 
 mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p
 
-# shellcheck disable=SC2154
-mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
 # shellcheck disable=SC2154
 mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -8,6 +8,14 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 
 	echo "INFO  [init] SIH simulator"
 
+	if [ -n "${PX4_HOME_LAT}" ]; then
+		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
+	fi
+
+	if [ -n "${PX4_HOME_LON}" ]; then
+		param set SIH_LOC_LON0 ${PX4_HOME_LON}
+	fi
+
 	if simulator_sih start; then
 
 		if param compare -s SENS_EN_BAROSIM 1

boards/holybro/kakuteh7mini/init/rc.board_defaults

@@ -22,13 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050
 
-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_ACC_COMP 0
-param set-default ATT_W_ACC 0.4000
-param set-default ATT_W_GYRO_BIAS 0.0000
-
+# use EKF2 without mag
 param set-default SYS_HAS_MAG 0
+# and enable gravity fusion
+param set-default EKF2_IMU_CONTROL 7
 
 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090
@@ -41,11 +38,5 @@ param set-default SYS_DM_BACKEND 1
 # Ignore that there is no SD card
 param set-default COM_ARM_SDCARD 0
 
-# Store missions in RAM
-param set-default SYS_DM_BACKEND 1
-
-# Ignore that there is no SD card
-param set-default COM_ARM_SDCARD 0
-
 # Don't try to log onto SD card
 param set-default SDLOG_MODE -1

boards/holybro/kakuteh7v2/init/rc.board_defaults

@@ -22,13 +22,10 @@ param set-default CBRK_SUPPLY_CHK 894281
 # Select the Generic 250 Racer by default
 param set-default SYS_AUTOSTART 4050
 
-# use the Q attitude estimator, it works w/o mag or GPS.
-param set-default SYS_MC_EST_GROUP 3
-param set-default ATT_ACC_COMP 0
-param set-default ATT_W_ACC 0.4000
-param set-default ATT_W_GYRO_BIAS 0.0000
-
+# use EKF2 without mag
 param set-default SYS_HAS_MAG 0
+# and enable gravity fusion
+param set-default EKF2_IMU_CONTROL 7
 
 # the startup tune is not great on a binary output buzzer, so disable it
 param set-default CBRK_BUZZER 782090

msg/FailsafeFlags.msg

@@ -17,6 +17,7 @@ uint32 mode_req_offboard_signal
 uint32 mode_req_home_position
 uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded
 uint32 mode_req_prevent_arming    # if set, cannot arm while in this mode
+uint32 mode_req_manual_control
 uint32 mode_req_other             # other requirements, not covered above (for external modes)
 
 

platforms/common/uORB/uORB.cpp

@@ -347,12 +347,12 @@ void orb_print_message_internal(const orb_metadata *meta, const void *data, bool
 					data_offset += sizeof(uint64_t);
 
 				} else if (strcmp(c_type, "float") == 0) {
-					if (!dont_print) { PX4_INFO_RAW("%.4f", (double) * (float *)(data_ptr + data_offset)); }
+					if (!dont_print) { PX4_INFO_RAW("%.5f", (double) * (float *)(data_ptr + data_offset)); }
 
 					data_offset += sizeof(float);
 
 				} else if (strcmp(c_type, "double") == 0) {
-					if (!dont_print) { PX4_INFO_RAW("%.4f", *(double *)(data_ptr + data_offset)); }
+					if (!dont_print) { PX4_INFO_RAW("%.6f", *(double *)(data_ptr + data_offset)); }
 
 					data_offset += sizeof(double);
 

platforms/posix/Debug/launch_sitl.json.in

@@ -14,11 +14,11 @@
             "environment": [
                 {
                     "name": "PX4_SIM_MODEL",
-                    "value": "${input:PX4_GZ_MODEL}"
+                    "value": "gz_${input:PX4_GZ_MODEL}"
                 }
             ],
             "externalConsole": false,
-            "postDebugTask": "ign gazebo kill",
+            "postDebugTask": "gazebo kill",
             "linux": {
                 "MIMode": "gdb",
                 "externalConsole": false,
@@ -222,6 +222,9 @@
             "description": "GZ vehicle model",
             "options": [
               "x500",
+              "x500_depth",
+              "rc_cessna",
+              "standard_vtol",
             ],
             "default": "x500"
         }

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -246,6 +246,8 @@ public:
 	void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id,
 				const events::LogLevels &log_levels, const char *message);
 
+	void clearArmingBits(NavModes modes);
+
 	/**
 	 * Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
 	 * mode is being run.
@@ -302,8 +304,6 @@ private:
 
 	NavModes reportedModes(NavModes required_modes);
 
-	void clearArmingBits(NavModes modes);
-
 	NavModes getModeGroup(uint8_t nav_state) const;
 
 	friend class HealthAndArmingChecks;

src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp

@@ -143,6 +143,26 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_home_position);
 	}
 
+	if (reporter.failsafeFlags().manual_control_signal_lost && reporter.failsafeFlags().mode_req_manual_control != 0) {
+		const bool rc_disabled = (_param_com_rc_in_mode.get() == 4);
+		NavModes nav_modes = rc_disabled ? (NavModes)reporter.failsafeFlags().mode_req_manual_control : NavModes::None;
+		events::LogLevel log_level = rc_disabled ? events::Log::Error : events::Log::Warning;
+
+		/* EVENT
+		 * @description
+		 * Connect and enable stick input or use autonomous mode.
+		 * <profile name="dev">
+		 * Sticks can be enabled via <param>COM_RC_IN_MODE</param> parameter.
+		 * </profile>
+		 */
+		reporter.armingCheckFailure(nav_modes,
+					    health_component_t::remote_control,
+					    events::ID("check_modes_manual_control"),
+					    log_level, "No manual control input");
+		reporter.clearArmingBits((NavModes)reporter.failsafeFlags().mode_req_manual_control);
+		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_manual_control);
+	}
+
 	if (reporter.failsafeFlags().mode_req_other != 0) {
 		// Here we expect there is already an event reported for the failing check (this is for external modes)
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_other);

src/modules/commander/HealthAndArmingChecks/checks/modeCheck.hpp

@@ -49,6 +49,7 @@ private:
 	void checkArmingRequirement(const Context &context, Report &reporter);
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req
+					(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
+					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode
 				       );
 };

src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp

@@ -38,65 +38,40 @@ using namespace time_literals;
 void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	// RC
-	bool rc_is_optional = true;
+	manual_control_setpoint_s manual_control_setpoint;
 
-	if (_param_com_rc_in_mode.get() == 4) { // RC disabled
-		reporter.failsafeFlags().manual_control_signal_lost = false;
+	if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+		manual_control_setpoint = {};
+		reporter.failsafeFlags().manual_control_signal_lost = true;
+	}
+
+	// Check if RC is valid
+	if (!manual_control_setpoint.valid
+	    || hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
+
+		if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
+
+			events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
+				     "Manual control lost");
+		}
+
+		reporter.failsafeFlags().manual_control_signal_lost = true;
 
 	} else {
+		reporter.setIsPresent(health_component_t::remote_control);
 
-		manual_control_setpoint_s manual_control_setpoint;
-
-		if (!_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-			manual_control_setpoint = {};
-			reporter.failsafeFlags().manual_control_signal_lost = true;
+		if (reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
+			float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
+			events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
+					    "Manual control regained after {1:.1} s", elapsed);
 		}
 
-		// Check if RC is valid
-		if (!manual_control_setpoint.valid
-		    || hrt_elapsed_time(&manual_control_setpoint.timestamp) > _param_com_rc_loss_t.get() * 1_s) {
-
-			if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
-
-				events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
-					     "Manual control lost");
-			}
-
-			reporter.failsafeFlags().manual_control_signal_lost = true;
-
-		} else {
-			reporter.setIsPresent(health_component_t::remote_control);
-
-			if (reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) {
-				float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
-				events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
-						    "Manual control regained after {1:.1} s", elapsed);
-			}
-
-			reporter.failsafeFlags().manual_control_signal_lost = false;
-			_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
-		}
-
-
-		if (reporter.failsafeFlags().manual_control_signal_lost) {
-
-			NavModes affected_modes = rc_is_optional ? NavModes::None : NavModes::All;
-			events::LogLevel log_level = rc_is_optional ? events::Log::Info : events::Log::Error;
-			/* EVENT
-			 * @description
-			 * <profile name="dev">
-			 * This check can be configured via <param>COM_RC_IN_MODE</param> parameter.
-			 * </profile>
-			 */
-			reporter.armingCheckFailure(affected_modes, health_component_t::remote_control, events::ID("check_rc_dl_no_rc"),
-						    log_level, "No manual control input");
-
-			if (reporter.mavlink_log_pub()) {
-				mavlink_log_info(reporter.mavlink_log_pub(), "Preflight Fail: No manual control input\t");
-			}
-		}
+		reporter.failsafeFlags().manual_control_signal_lost = false;
+		_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
 	}
 
+	// Manual control check is in modeCheck as mode requirement
+
 	// GCS connection
 	reporter.failsafeFlags().gcs_connection_lost = context.status().gcs_connection_lost;
 

src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.hpp

@@ -52,7 +52,6 @@ private:
 	hrt_abstime	_last_valid_manual_control_setpoint{0};
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
 					(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
 					(ParamInt<px4::params::NAV_DLL_ACT>) _param_nav_dll_act
 				       )

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -56,26 +56,29 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	flags.mode_req_home_position = 0;
 	flags.mode_req_wind_and_flight_time_compliance = 0;
 	flags.mode_req_prevent_arming = 0;
+	flags.mode_req_manual_control = 0;
 	flags.mode_req_other = 0;
 
 	// NAVIGATION_STATE_MANUAL
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_MANUAL, flags.mode_req_manual_control);
 
 	// NAVIGATION_STATE_ALTCTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_attitude);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_local_alt);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ALTCTL, flags.mode_req_manual_control);
 
 	// NAVIGATION_STATE_POSCTL
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_alt);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_manual_control);
 
 	if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
 	}
 
-	setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_alt);
-
 	// NAVIGATION_STATE_AUTO_MISSION
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);
@@ -104,6 +107,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 
 	// NAVIGATION_STATE_ACRO
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_angular_velocity);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_ACRO, flags.mode_req_manual_control);
 
 	// NAVIGATION_STATE_DESCEND
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_DESCEND, flags.mode_req_angular_velocity);
@@ -122,6 +126,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	// NAVIGATION_STATE_STAB
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_angular_velocity);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_attitude);
+	setRequirement(vehicle_status_s::NAVIGATION_STATE_STAB, flags.mode_req_manual_control);
 
 	// NAVIGATION_STATE_AUTO_TAKEOFF
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_angular_velocity);

src/modules/commander/failsafe/framework.cpp

@@ -656,6 +656,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
 		(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
 		(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
 		(!status_flags.home_position_invalid || ((status_flags.mode_req_home_position & mode_mask) == 0)) &&
+		(!status_flags.manual_control_signal_lost || ((status_flags.mode_req_manual_control & mode_mask) == 0)) &&
 		((status_flags.mode_req_other & mode_mask) == 0);
 }
 

src/modules/ekf2/EKF/common.h

@@ -355,7 +355,7 @@ struct parameters {
 	int32_t mag_declination_source{7};      ///< bitmask used to control the handling of declination data
 	int32_t mag_fusion_type{0};             ///< integer used to specify the type of magnetometer fusion used
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
-	float mag_yaw_rate_gate{0.25f};         ///< yaw rate threshold used by mode select logic (rad/sec)
+	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)
 
 	// GNSS heading fusion
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)

src/modules/ekf2/EKF/ekf.h

@@ -769,20 +769,47 @@ private:
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
 
-	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
+	void clearInhibitedStateKalmanGains(Vector24f &K) const
 	{
+		// gyro bias: states 10, 11, 12
 		for (unsigned i = 0; i < 3; i++) {
-			// gyro bias: states 10, 11, 12
 			if (_gyro_bias_inhibit[i]) {
-				K(10 + i) = 0.0f;
-			}
-
-			// accel bias: states 13, 14, 15
-			if (_accel_bias_inhibit[i]) {
-				K(13 + i) = 0.0f;
+				K(10 + i) = 0.f;
 			}
 		}
 
+		// accel bias: states 13, 14, 15
+		for (unsigned i = 0; i < 3; i++) {
+			if (_accel_bias_inhibit[i]) {
+				K(13 + i) = 0.f;
+			}
+		}
+
+		// mag I: states 16, 17, 18
+		if (!_control_status.flags.mag_3D) {
+			K(16) = 0.f;
+			K(17) = 0.f;
+			K(18) = 0.f;
+		}
+
+		// mag B: states 19, 20, 21
+		if (!_control_status.flags.mag_3D) {
+			K(19) = 0.f;
+			K(20) = 0.f;
+			K(21) = 0.f;
+		}
+
+		// wind: states 22, 23
+		if (!_control_status.flags.wind) {
+			K(22) = 0.f;
+			K(23) = 0.f;
+		}
+	}
+
+	bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation)
+	{
+		clearInhibitedStateKalmanGains(K);
+
 		const Vector24f KS = K * innovation_variance;
 		SquareMatrix24f KHP;
 
@@ -863,11 +890,10 @@ private:
 	// control fusion of magnetometer observations
 	void controlMagFusion();
 
-	void checkHaglYawResetReq();
 	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
 
-	void runOnGroundYawReset();
-	void runInAirYawReset();
+	bool magReset();
+	bool haglYawResetReq();
 
 	void selectMagAuto();
 	void check3DMagFusionSuitability();

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -41,7 +41,7 @@
  */
 
 #include "ekf.h"
-#include "python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h"
+#include "python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h"
 
 #include <mathlib/mathlib.h>
 #include <cstdlib>
@@ -59,17 +59,17 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample)
 
 		const float R_YAW = sq(fmaxf(gps_sample.yaw_acc, _params.gps_heading_noise));
 
-		float heading_innov;
+		float heading_pred;
 		float heading_innov_var;
 
 		{
 		Vector24f H;
-		sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
+		sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
 		}
 
 		gnss_yaw.observation = measured_hdg;
 		gnss_yaw.observation_variance = R_YAW;
-		gnss_yaw.innovation = heading_innov;
+		gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
 		gnss_yaw.innovation_variance = heading_innov_var;
 
 		gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
@@ -93,12 +93,12 @@ void Ekf::fuseGpsYaw()
 	Vector24f H;
 
 	{
-	float heading_innov;
+	float heading_pred;
 	float heading_innov_var;
 
 	// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
 	// making a separate function just for H uses more flash space without reducing CPU load significantly
-	sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation, gnss_yaw.observation_variance, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
 	}
 
 	const SparseVector24f<0,1,2,3> Hfusion(H);

src/modules/ekf2/EKF/mag_control.cpp

@@ -206,18 +206,18 @@ void Ekf::controlMagFusion()
 			break;
 		}
 
-		const bool mag_enabled = _control_status.flags.mag_hdg || _control_status.flags.mag_3D;
+		if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
 
-		if ((!mag_enabled_previously && mag_enabled) || mag_sample.reset) {
-			_mag_yaw_reset_req = true;
-		}
+			if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) {
 
-		if (_control_status.flags.in_air) {
-			checkHaglYawResetReq();
-			runInAirYawReset();
+				if (magReset()) {
+					_mag_yaw_reset_req = false;
 
-		} else {
-			runOnGroundYawReset();
+				} else {
+					// mag reset failed, try again next time
+					_mag_yaw_reset_req = true;
+				}
+			}
 		}
 
 		if (!_control_status.flags.yaw_align) {
@@ -231,90 +231,84 @@ void Ekf::controlMagFusion()
 	}
 }
 
-void Ekf::checkHaglYawResetReq()
+bool Ekf::haglYawResetReq()
 {
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
-	if (!_control_status.flags.mag_aligned_in_flight) {
+	if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
 		// Check if height has increased sufficiently to be away from ground magnetic anomalies
 		// and request a yaw reset if not already requested.
 		static constexpr float mag_anomalies_max_hagl = 1.5f;
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
-		_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
+		return above_mag_anomalies;
 	}
+
+	return false;
 }
 
-void Ekf::runOnGroundYawReset()
-{
-	if (_mag_yaw_reset_req) {
-		const bool has_realigned_yaw = resetMagHeading();
-
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
-		}
-	}
-}
-
-void Ekf::runInAirYawReset()
+bool Ekf::magReset()
 {
 	// prevent a reset being performed more than once on the same frame
 	if ((_flt_mag_align_start_time == _time_delayed_us)
 	    || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) {
-		return;
+		return false;
 	}
 
-	if (_mag_yaw_reset_req) {
-		bool has_realigned_yaw = false;
+	bool has_realigned_yaw = false;
 
-		// use yaw estimator if available
-		if (_control_status.flags.gps && isYawEmergencyEstimateAvailable()
-		    && (_mag_counter > 1) // mag LPF available
-		   ) {
+	// use yaw estimator if available
+	if (_control_status.flags.gps && isYawEmergencyEstimateAvailable()
+	    && (_mag_counter > 1) // mag LPF available
+	   ) {
 
-			resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
+		resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
-			_information_events.flags.yaw_aligned_to_imu_gps = true;
+		_information_events.flags.yaw_aligned_to_imu_gps = true;
 
-			// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-			if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
-				// use predicted earth field to reset states
-				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
-				_state.mag_I = mag_earth_pred;
+		// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
+		if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+			// use predicted earth field to reset states
+			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+			_state.mag_I = mag_earth_pred;
 
-				const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-				_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			_state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred);
 
-			} else {
-				// Use the last magnetometer measurements to reset the field states
-				// calculate initial earth magnetic field states
-				_state.mag_I = _R_to_earth * _mag_lpf.getState();
-				_state.mag_B.zero();
-			}
-
-			ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
-				(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
-				(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
-				);
-
-			resetMagCov();
-
-			has_realigned_yaw = true;
+		} else {
+			// Use the last magnetometer measurements to reset the field states
+			// calculate initial earth magnetic field states
+			_state.mag_I = _R_to_earth * _mag_lpf.getState();
+			_state.mag_B.zero();
 		}
 
-		if (!has_realigned_yaw) {
-			has_realigned_yaw = resetMagHeading();
-		}
+		ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]",
+			(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
+			(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
+			);
 
-		if (has_realigned_yaw) {
-			_mag_yaw_reset_req = false;
-			_control_status.flags.yaw_align = true;
+		resetMagCov();
+
+		has_realigned_yaw = true;
+	}
+
+	if (!has_realigned_yaw) {
+		has_realigned_yaw = resetMagHeading();
+	}
+
+	if (has_realigned_yaw) {
+		_control_status.flags.yaw_align = true;
+
+		if (_control_status.flags.in_air) {
 			_control_status.flags.mag_aligned_in_flight = true;
 
 			// record the time for the magnetic field alignment event
 			_flt_mag_align_start_time = _time_delayed_us;
 		}
+
+		return true;
 	}
+
+	return false;
 }
 
 void Ekf::selectMagAuto()
@@ -452,9 +446,11 @@ void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 
 void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
-	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
-	// before they are used to constrain heading drift
-	const bool update_all_states = ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)5e6);
+	// sanity check mag_B before they are used to constrain heading drift
+	const Vector3f mag_bias_var = P.slice<3, 3>(19, 19).diag();
+	const bool mag_bias_var_good = (mag_bias_var.min() > 0.f) && (mag_bias_var.max() < sq(0.02f));
+
+	const bool update_all_states = _control_status.flags.mag_aligned_in_flight && mag_bias_var_good;
 
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -258,7 +258,7 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 	// only calculate gains for states we are using
 	Vector24f Kfusion;
 
-	for (uint8_t row = 0; row <= 15; row++) {
+	for (uint8_t row = 0; row < _k_num_states; row++) {
 		for (uint8_t col = 0; col <= 3; col++) {
 			Kfusion(row) += P(row, col) * H_YAW(col);
 		}
@@ -266,16 +266,6 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
 		Kfusion(row) *= heading_innov_var_inv;
 	}
 
-	if (_control_status.flags.wind) {
-		for (uint8_t row = 22; row <= 23; row++) {
-			for (uint8_t col = 0; col <= 3; col++) {
-				Kfusion(row) += P(row, col) * H_YAW(col);
-			}
-
-			Kfusion(row) *= heading_innov_var_inv;
-		}
-	}
-
 	// define the innovation gate size
 	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
 

src/modules/ekf2/EKF/python/ekf_derivation/derivation.py

@@ -387,11 +387,10 @@ def compute_flow_y_innov_var_and_h(
 
     return (innov_var, Hy.T)
 
-def compute_gnss_yaw_innon_innov_var_and_h(
+def compute_gnss_yaw_pred_innov_var_and_h(
         state: VState,
         P: MState,
         antenna_yaw_offset: sf.Scalar,
-        meas: sf.Scalar,
         R: sf.Scalar,
         epsilon: sf.Scalar
 ) -> (sf.Scalar, sf.Scalar, VState):
@@ -411,9 +410,7 @@ def compute_gnss_yaw_innon_innov_var_and_h(
     H = sf.V1(meas_pred).jacobian(state)
     innov_var = (H * P * H.T + R)[0,0]
 
-    innov = meas_pred - meas
-
-    return (innov, innov_var, H.T)
+    return (meas_pred, innov_var, H.T)
 
 def predict_drag(
         state: VState,
@@ -524,7 +521,7 @@ generate_px4_function(compute_yaw_312_innov_var_and_h_alternate, output_names=["
 generate_px4_function(compute_mag_declination_innov_innov_var_and_h, output_names=["innov", "innov_var", "H"])
 generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
 generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
-generate_px4_function(compute_gnss_yaw_innon_innov_var_and_h, output_names=["innov", "innov_var", "H"])
+generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"])
 generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"])
 generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"])

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h

@@ -1,103 +0,0 @@
-// -----------------------------------------------------------------------------
-// This file was autogenerated by symforce from template:
-//     backends/cpp/templates/function/FUNCTION.h.jinja
-// Do NOT modify by hand.
-// -----------------------------------------------------------------------------
-
-#pragma once
-
-#include <matrix/math.hpp>
-
-namespace sym {
-
-/**
- * This function was autogenerated from a symbolic function. Do not modify by hand.
- *
- * Symbolic function: compute_gnss_yaw_innon_innov_var_and_h
- *
- * Args:
- *     state: Matrix24_1
- *     P: Matrix24_24
- *     antenna_yaw_offset: Scalar
- *     meas: Scalar
- *     R: Scalar
- *     epsilon: Scalar
- *
- * Outputs:
- *     innov: Scalar
- *     innov_var: Scalar
- *     H: Matrix24_1
- */
-template <typename Scalar>
-void ComputeGnssYawInnonInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
-                                     const matrix::Matrix<Scalar, 24, 24>& P,
-                                     const Scalar antenna_yaw_offset, const Scalar meas,
-                                     const Scalar R, const Scalar epsilon,
-                                     Scalar* const innov = nullptr,
-                                     Scalar* const innov_var = nullptr,
-                                     matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
-  // Total ops: 106
-
-  // Input arrays
-
-  // Intermediate terms (28)
-  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
-  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
-  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
-  const Scalar _tmp3 = std::sin(antenna_yaw_offset);
-  const Scalar _tmp4 = state(0, 0) * state(3, 0);
-  const Scalar _tmp5 = state(1, 0) * state(2, 0);
-  const Scalar _tmp6 = std::cos(antenna_yaw_offset);
-  const Scalar _tmp7 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + 2 * _tmp6 * (_tmp4 + _tmp5);
-  const Scalar _tmp8 = 2 * _tmp3 * (-_tmp4 + _tmp5) + _tmp6 * (-_tmp0 + _tmp1 + _tmp2);
-  const Scalar _tmp9 = _tmp8 + epsilon * ((((_tmp8) > 0) - ((_tmp8) < 0)) + Scalar(0.5));
-  const Scalar _tmp10 = 2 * state(3, 0);
-  const Scalar _tmp11 = 2 * state(0, 0);
-  const Scalar _tmp12 = -_tmp10 * _tmp3 + _tmp11 * _tmp6;
-  const Scalar _tmp13 = Scalar(1.0) / (_tmp9);
-  const Scalar _tmp14 = _tmp10 * _tmp6;
-  const Scalar _tmp15 = _tmp11 * _tmp3;
-  const Scalar _tmp16 = std::pow(_tmp9, Scalar(2));
-  const Scalar _tmp17 = _tmp7 / _tmp16;
-  const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp7, Scalar(2)));
-  const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15));
-  const Scalar _tmp20 = 2 * state(1, 0);
-  const Scalar _tmp21 = 2 * state(2, 0);
-  const Scalar _tmp22 = _tmp20 * _tmp6 + _tmp21 * _tmp3;
-  const Scalar _tmp23 = _tmp20 * _tmp3;
-  const Scalar _tmp24 = _tmp21 * _tmp6;
-  const Scalar _tmp25 = _tmp18 * (_tmp13 * (-_tmp23 + _tmp24) - _tmp17 * _tmp22);
-  const Scalar _tmp26 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15));
-  const Scalar _tmp27 = _tmp18 * (_tmp13 * _tmp22 - _tmp17 * (_tmp23 - _tmp24));
-
-  // Output terms (3)
-  if (innov != nullptr) {
-    Scalar& _innov = (*innov);
-
-    _innov = -meas + std::atan2(_tmp7, _tmp9);
-  }
-
-  if (innov_var != nullptr) {
-    Scalar& _innov_var = (*innov_var);
-
-    _innov_var =
-        R + _tmp19 * (P(0, 3) * _tmp26 + P(1, 3) * _tmp25 + P(2, 3) * _tmp27 + P(3, 3) * _tmp19) +
-        _tmp25 * (P(0, 1) * _tmp26 + P(1, 1) * _tmp25 + P(2, 1) * _tmp27 + P(3, 1) * _tmp19) +
-        _tmp26 * (P(0, 0) * _tmp26 + P(1, 0) * _tmp25 + P(2, 0) * _tmp27 + P(3, 0) * _tmp19) +
-        _tmp27 * (P(0, 2) * _tmp26 + P(1, 2) * _tmp25 + P(2, 2) * _tmp27 + P(3, 2) * _tmp19);
-  }
-
-  if (H != nullptr) {
-    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
-
-    _h.setZero();
-
-    _h(0, 0) = _tmp26;
-    _h(1, 0) = _tmp25;
-    _h(2, 0) = _tmp27;
-    _h(3, 0) = _tmp19;
-  }
-}  // NOLINT(readability/fn_size)
-
-// NOLINTNEXTLINE(readability/fn_size)
-}  // namespace sym

src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h

@@ -0,0 +1,99 @@
+// -----------------------------------------------------------------------------
+// This file was autogenerated by symforce from template:
+//     backends/cpp/templates/function/FUNCTION.h.jinja
+// Do NOT modify by hand.
+// -----------------------------------------------------------------------------
+
+#pragma once
+
+#include <matrix/math.hpp>
+
+namespace sym {
+
+/**
+ * This function was autogenerated from a symbolic function. Do not modify by hand.
+ *
+ * Symbolic function: compute_gnss_yaw_pred_innov_var_and_h
+ *
+ * Args:
+ *     state: Matrix24_1
+ *     P: Matrix24_24
+ *     antenna_yaw_offset: Scalar
+ *     R: Scalar
+ *     epsilon: Scalar
+ *
+ * Outputs:
+ *     meas_pred: Scalar
+ *     innov_var: Scalar
+ *     H: Matrix24_1
+ */
+template <typename Scalar>
+void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix<Scalar, 24, 1>& state,
+                                    const matrix::Matrix<Scalar, 24, 24>& P,
+                                    const Scalar antenna_yaw_offset, const Scalar R,
+                                    const Scalar epsilon, Scalar* const meas_pred = nullptr,
+                                    Scalar* const innov_var = nullptr,
+                                    matrix::Matrix<Scalar, 24, 1>* const H = nullptr) {
+  // Total ops: 101
+
+  // Input arrays
+
+  // Intermediate terms (26)
+  const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2));
+  const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2));
+  const Scalar _tmp2 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2));
+  const Scalar _tmp3 = std::sin(antenna_yaw_offset);
+  const Scalar _tmp4 = state(0, 0) * state(3, 0);
+  const Scalar _tmp5 = state(1, 0) * state(2, 0);
+  const Scalar _tmp6 = std::cos(antenna_yaw_offset);
+  const Scalar _tmp7 = 2 * _tmp6;
+  const Scalar _tmp8 = _tmp3 * (_tmp0 - _tmp1 + _tmp2) + _tmp7 * (_tmp4 + _tmp5);
+  const Scalar _tmp9 = 2 * _tmp3;
+  const Scalar _tmp10 = _tmp6 * (-_tmp0 + _tmp1 + _tmp2) + _tmp9 * (-_tmp4 + _tmp5);
+  const Scalar _tmp11 = _tmp10 + epsilon * ((((_tmp10) > 0) - ((_tmp10) < 0)) + Scalar(0.5));
+  const Scalar _tmp12 = _tmp7 * state(0, 0) - _tmp9 * state(3, 0);
+  const Scalar _tmp13 = Scalar(1.0) / (_tmp11);
+  const Scalar _tmp14 = _tmp7 * state(3, 0);
+  const Scalar _tmp15 = _tmp9 * state(0, 0);
+  const Scalar _tmp16 = std::pow(_tmp11, Scalar(2));
+  const Scalar _tmp17 = _tmp8 / _tmp16;
+  const Scalar _tmp18 = _tmp16 / (_tmp16 + std::pow(_tmp8, Scalar(2)));
+  const Scalar _tmp19 = _tmp18 * (_tmp12 * _tmp13 - _tmp17 * (-_tmp14 - _tmp15));
+  const Scalar _tmp20 = _tmp7 * state(1, 0) + _tmp9 * state(2, 0);
+  const Scalar _tmp21 = _tmp9 * state(1, 0);
+  const Scalar _tmp22 = _tmp7 * state(2, 0);
+  const Scalar _tmp23 = _tmp18 * (_tmp13 * (-_tmp21 + _tmp22) - _tmp17 * _tmp20);
+  const Scalar _tmp24 = _tmp18 * (-_tmp12 * _tmp17 + _tmp13 * (_tmp14 + _tmp15));
+  const Scalar _tmp25 = _tmp18 * (_tmp13 * _tmp20 - _tmp17 * (_tmp21 - _tmp22));
+
+  // Output terms (3)
+  if (meas_pred != nullptr) {
+    Scalar& _meas_pred = (*meas_pred);
+
+    _meas_pred = std::atan2(_tmp8, _tmp11);
+  }
+
+  if (innov_var != nullptr) {
+    Scalar& _innov_var = (*innov_var);
+
+    _innov_var =
+        R + _tmp19 * (P(0, 3) * _tmp24 + P(1, 3) * _tmp23 + P(2, 3) * _tmp25 + P(3, 3) * _tmp19) +
+        _tmp23 * (P(0, 1) * _tmp24 + P(1, 1) * _tmp23 + P(2, 1) * _tmp25 + P(3, 1) * _tmp19) +
+        _tmp24 * (P(0, 0) * _tmp24 + P(1, 0) * _tmp23 + P(2, 0) * _tmp25 + P(3, 0) * _tmp19) +
+        _tmp25 * (P(0, 2) * _tmp24 + P(1, 2) * _tmp23 + P(2, 2) * _tmp25 + P(3, 2) * _tmp19);
+  }
+
+  if (H != nullptr) {
+    matrix::Matrix<Scalar, 24, 1>& _h = (*H);
+
+    _h.setZero();
+
+    _h(0, 0) = _tmp24;
+    _h(1, 0) = _tmp23;
+    _h(2, 0) = _tmp25;
+    _h(3, 0) = _tmp19;
+  }
+}  // NOLINT(readability/fn_size)
+
+// NOLINTNEXTLINE(readability/fn_size)
+}  // namespace sym

src/modules/ekf2/EKF/vel_pos_fusion.cpp

@@ -200,17 +200,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 		Kfusion(row) = P(row, state_index) / innov_var;
 	}
 
-	for (unsigned i = 0; i < 3; i++) {
-		// gyro bias: states 10, 11, 12
-		if (_gyro_bias_inhibit[i]) {
-			Kfusion(10 + i) = 0.0f;
-		}
-
-		// accel bias: states 13, 14, 15
-		if (_accel_bias_inhibit[i]) {
-			Kfusion(13 + i) = 0.0f;
-		}
-	}
+	clearInhibitedStateKalmanGains(Kfusion);
 
 	SquareMatrix24f KHP;
 

src/modules/ekf2/EKF2.cpp

@@ -2283,8 +2283,11 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
-	const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D)
+	const bool bias_valid = (_param_ekf2_mag_type.get() == static_cast<int32_t>(MagFuseType::AUTO)
+				 || _param_ekf2_mag_type.get() == static_cast<int32_t>(MagFuseType::MAG_3D))
+				&& _ekf.control_status_flags().yaw_align
 				&& _ekf.control_status_flags().mag_aligned_in_flight
+				&& (_ekf.fault_status().value == 0)
 				&& !_ekf.control_status_flags().mag_fault
 				&& !_ekf.control_status_flags().mag_field_disturbed;
 
@@ -2294,7 +2297,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 			  bias_valid, learning_valid);
 
 	// update stored declination value
-	if (!_mag_decl_saved) {
+	if (!_mag_decl_saved && !_ekf.control_status_flags().in_air) {
 		float declination_deg;
 
 		if (_ekf.get_mag_decl_deg(&declination_deg)) {

src/modules/ekf2/ekf2_params.c

@@ -532,7 +532,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f);
  * @unit rad/s
  * @decimal 2
  */
-PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.25f);
+PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.20f);
 
 /**
  * Gate size for barometric and GPS height fusion

src/modules/ekf2/test/test_EKF_gnss_yaw_generated.cpp

@@ -35,7 +35,7 @@
 #include "EKF/ekf.h"
 #include "test_helper/comparison_helper.h"
 
-#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_innon_innov_var_and_h.h"
+#include "../EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h"
 
 using namespace matrix;
 
@@ -140,11 +140,11 @@ TEST(GnssYawFusionGenerated, SympyVsSymforce)
 	Vector24f K_sympy;
 	sympyGnssYawInnovVarHAndK(q(0), q(1), q(2), q(3), P, yaw_offset, R_YAW, innov_var_sympy, H_sympy, K_sympy);
 
-	float innov_symforce;
+	float meas_pred_symforce;
 	float innov_var_symforce;
 	Vector24f H_symforce;
-	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov_symforce,
-					     &innov_var_symforce, &H_symforce);
+	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred_symforce,
+					    &innov_var_symforce, &H_symforce);
 
 	// K isn't generated from symbolic anymore to save flash space
 	Vector24f K_symforce = P * H_symforce / innov_var_symforce;
@@ -177,11 +177,11 @@ TEST(GnssYawFusionGenerated, SingularityPitch90)
 	SquareMatrix24f P = createRandomCovarianceMatrix24f();
 	const float R_YAW = sq(0.3f);
 
-	float innov;
+	float meas_pred;
 	float innov_var;
 	Vector24f H;
-	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov,
-					     &innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
+					    &innov_var, &H);
 	Vector24f K = P * H / innov_var;
 
 	// THEN: the arctan is singular, the attitude isn't observable, so the innovation variance
@@ -205,11 +205,11 @@ TEST(GnssYawFusionGenerated, SingularityRoll90)
 	SquareMatrix24f P = createRandomCovarianceMatrix24f();
 	const float R_YAW = sq(0.3f);
 
-	float innov;
+	float meas_pred;
 	float innov_var;
 	Vector24f H;
-	sym::ComputeGnssYawInnonInnovVarAndH(state_vector, P, yaw_offset, 0.f, R_YAW, FLT_EPSILON, &innov,
-					     &innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(state_vector, P, yaw_offset, R_YAW, FLT_EPSILON, &meas_pred,
+					    &innov_var, &H);
 	Vector24f K = P * H / innov_var;
 
 	// THEN: the arctan is singular, the attitude isn't observable, so the innovation variance

src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp

@@ -52,9 +52,7 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se
 		_stick_acceleration_xy.resetVelocity(_velocity.xy());
 	}
 
-	if (Vector2f(last_setpoint.acceleration).isAllFinite()) {
-		_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
-	}
+	_stick_acceleration_xy.resetAcceleration(Vector2f(last_setpoint.acceleration));
 
 	return ret;
 }

src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp

@@ -86,7 +86,6 @@ protected:
 					(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy,
 					(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p, /**< position controller altitude propotional gain */
-					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
 					(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1, /**< altitude at which to start downwards slowdown */
 					(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2, /**< altitude below which to land with land speed */
 					(ParamFloat<px4::params::MPC_LAND_SPEED>)

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp

@@ -57,13 +57,17 @@ void StickAccelerationXY::resetPosition(const matrix::Vector2f &position)
 
 void StickAccelerationXY::resetVelocity(const matrix::Vector2f &velocity)
 {
-	_velocity_setpoint = velocity;
+	if (velocity.isAllFinite()) {
+		_velocity_setpoint = velocity;
+	}
 }
 
 void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration)
 {
-	_acceleration_slew_rate_x.setForcedValue(acceleration(0));
-	_acceleration_slew_rate_y.setForcedValue(acceleration(1));
+	if (acceleration.isAllFinite()) {
+		_acceleration_slew_rate_x.setForcedValue(acceleration(0));
+		_acceleration_slew_rate_y.setForcedValue(acceleration(1));
+	}
 }
 
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
@@ -150,7 +154,7 @@ Vector2f StickAccelerationXY::calculateDrag(Vector2f drag_coefficient, const flo
 
 	drag_coefficient *= _brake_boost_filter.getState();
 
-	// increase drag with sqareroot function when velocity is lower than 1m/s
+	// increase drag with squareroot function when velocity is lower than 1m/s
 	const Vector2f velocity_with_sqrt_boost = vel_sp.unit_or_zero() * math::sqrt_linear(vel_sp.norm());
 	return drag_coefficient.emult(velocity_with_sqrt_boost);
 }

src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp

@@ -84,7 +84,6 @@ private:
 		(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
 		(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
 		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
-		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
-		(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
+		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max
 	)
 };

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -75,7 +75,7 @@ void LandingTargetEstimator::update()
 	/* predict */
 	if (_estimator_initialized) {
 		if (hrt_absolute_time() - _last_update > landing_target_estimator_TIMEOUT_US) {
-			PX4_WARN("Timeout");
+			PX4_INFO("Lost sight of Marker");
 			_estimator_initialized = false;
 
 		} else {
@@ -129,7 +129,7 @@ void LandingTargetEstimator::update()
 		if (!update_x || !update_y) {
 			if (!_faulty) {
 				_faulty = true;
-				PX4_WARN("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
+				PX4_INFO("Landing target measurement rejected:%s%s", update_x ? "" : " x", update_y ? "" : " y");
 			}
 
 		} else {
@@ -264,7 +264,7 @@ void LandingTargetEstimator::_update_topics()
 		}
 
 		if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) {
-			PX4_WARN("Position is corrupt!");
+			PX4_WARN("Marker position reading invalid!");
 			return;
 		}
 

src/modules/mavlink/Kconfig

@@ -8,8 +8,15 @@ menuconfig MAVLINK_DIALECT
 depends on MODULES_MAVLINK
         string "Mavlink dialect"
         default "common"
-    help
-        Select the Mavlink dialect to generate and use.
+	---help---
+		Select the Mavlink dialect to generate and use.
+
+menuconfig MAVLINK_UAVCAN_PARAMETERS
+depends on MODULES_MAVLINK && DRIVERS_UAVCAN
+        bool "Mavlink UAVCAN parameter support"
+        default y
+	---help---
+		Expose UAVCAN parameters over Mavlink.
 
 menuconfig USER_MAVLINK
 	bool "mavlink running as userspace module"

src/modules/mavlink/mavlink_parameters.cpp

@@ -77,6 +77,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				}
 			}
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 			if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 &&
 			    (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
 				// publish list request to UAVCAN driver via uORB.
@@ -88,6 +90,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				_uavcan_parameter_request_pub.publish(req);
 			}
 
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 			break;
 		}
 
@@ -133,6 +136,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				}
 			}
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 			if (set.target_system == mavlink_system.sysid && set.target_component < 127 &&
 			    (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
@@ -158,6 +163,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				_uavcan_parameter_request_pub.publish(req);
 			}
 
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 			break;
 		}
 
@@ -208,6 +214,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				}
 			}
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 			if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 &&
 			    (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) {
 				// publish set request to UAVCAN driver via uORB.
@@ -224,6 +232,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				request_next_uavcan_parameter();
 			}
 
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
 			break;
 		}
 
@@ -328,18 +337,22 @@ MavlinkParametersManager::send()
 bool
 MavlinkParametersManager::send_params()
 {
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
 	if (send_uavcan()) {
 		return true;
+	}
 
-	} else if (send_one()) {
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
+	if (send_one()) {
 		return true;
 
 	} else if (send_untransmitted()) {
 		return true;
-
-	} else {
-		return false;
 	}
+
+	return false;
 }
 
 bool
@@ -393,63 +406,6 @@ MavlinkParametersManager::send_untransmitted()
 	return sent_one;
 }
 
-bool
-MavlinkParametersManager::send_uavcan()
-{
-	/* Send parameter values received from the UAVCAN topic */
-	uavcan_parameter_value_s value{};
-
-	if (_uavcan_parameter_value_sub.update(&value)) {
-
-		// Check if we received a matching parameter, drop it from the list and request the next
-		if ((_uavcan_open_request_list != nullptr)
-		    && (value.param_index == _uavcan_open_request_list->req.param_index)
-		    && (value.node_id == _uavcan_open_request_list->req.node_id)) {
-
-			dequeue_uavcan_request();
-			request_next_uavcan_parameter();
-		}
-
-		mavlink_param_value_t msg{};
-		msg.param_count = value.param_count;
-		msg.param_index = value.param_index;
-#if defined(__GNUC__) && __GNUC__ >= 8
-#pragma GCC diagnostic ignored "-Wstringop-truncation"
-#endif
-		/*
-		 * coverity[buffer_size_warning : FALSE]
-		 *
-		 * The MAVLink spec does not require the string to be NUL-terminated if it
-		 * has length 16. In this case the receiving end needs to terminate it
-		 * when copying it.
-		 */
-		strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-#if defined(__GNUC__) && __GNUC__ >= 8
-#pragma GCC diagnostic pop
-#endif
-
-		if (value.param_type == MAV_PARAM_TYPE_REAL32) {
-			msg.param_type = MAVLINK_TYPE_FLOAT;
-			msg.param_value = value.real_value;
-
-		} else {
-			int32_t val = (int32_t)value.int_value;
-			memcpy(&msg.param_value, &val, sizeof(int32_t));
-			msg.param_type = MAVLINK_TYPE_INT32_T;
-		}
-
-		// Re-pack the message with the UAVCAN node ID
-		mavlink_message_t mavlink_packet{};
-		mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
-						    &msg);
-		_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
-
-		return true;
-	}
-
-	return false;
-}
-
 bool
 MavlinkParametersManager::send_one()
 {
@@ -591,6 +547,64 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
 	return 0;
 }
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+
+bool MavlinkParametersManager::send_uavcan()
+{
+	/* Send parameter values received from the UAVCAN topic */
+	uavcan_parameter_value_s value{};
+
+	if (_uavcan_parameter_value_sub.update(&value)) {
+
+		// Check if we received a matching parameter, drop it from the list and request the next
+		if ((_uavcan_open_request_list != nullptr)
+		    && (value.param_index == _uavcan_open_request_list->req.param_index)
+		    && (value.node_id == _uavcan_open_request_list->req.node_id)) {
+
+			dequeue_uavcan_request();
+			request_next_uavcan_parameter();
+		}
+
+		mavlink_param_value_t msg{};
+		msg.param_count = value.param_count;
+		msg.param_index = value.param_index;
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic ignored "-Wstringop-truncation"
+#endif
+		/*
+		 * coverity[buffer_size_warning : FALSE]
+		 *
+		 * The MAVLink spec does not require the string to be NUL-terminated if it
+		 * has length 16. In this case the receiving end needs to terminate it
+		 * when copying it.
+		 */
+		strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic pop
+#endif
+
+		if (value.param_type == MAV_PARAM_TYPE_REAL32) {
+			msg.param_type = MAVLINK_TYPE_FLOAT;
+			msg.param_value = value.real_value;
+
+		} else {
+			int32_t val = (int32_t)value.int_value;
+			memcpy(&msg.param_value, &val, sizeof(int32_t));
+			msg.param_type = MAVLINK_TYPE_INT32_T;
+		}
+
+		// Re-pack the message with the UAVCAN node ID
+		mavlink_message_t mavlink_packet{};
+		mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
+						    &msg);
+		_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
+
+		return true;
+	}
+
+	return false;
+}
+
 void MavlinkParametersManager::request_next_uavcan_parameter()
 {
 	// Request a parameter if we are not already waiting on a response and if the list is not empty
@@ -643,3 +657,5 @@ void MavlinkParametersManager::dequeue_uavcan_request()
 		_uavcan_waiting_for_request_response = false;
 	}
 }
+
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS

src/modules/mavlink/mavlink_parameters.h

@@ -49,11 +49,14 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/rc_parameter_map.h>
-#include <uORB/topics/uavcan_parameter_request.h>
-#include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+# include <uORB/topics/uavcan_parameter_request.h>
+# include <uORB/topics/uavcan_parameter_value.h>
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
 using namespace time_literals;
 
 class Mavlink;
@@ -91,11 +94,6 @@ protected:
 	 */
 	bool send_params();
 
-	/**
-	 * Send UAVCAN params
-	 */
-	bool send_uavcan();
-
 	/**
 	 * Send untransmitted params
 	 */
@@ -103,6 +101,12 @@ protected:
 
 	int send_param(param_t param, int component_id = -1);
 
+#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
+	/**
+	 * Send UAVCAN params
+	 */
+	bool send_uavcan();
+
 	// Item of a single-linked list to store requested uavcan parameters
 	struct _uavcan_open_request_list_item {
 		uavcan_parameter_request_s req;
@@ -128,9 +132,6 @@ protected:
 	bool _uavcan_waiting_for_request_response{false}; ///< We have reqested a parameter and wait for the response
 	uint16_t _uavcan_queued_request_items{0};	///< Number of stored parameter requests currently in the list
 
-	uORB::Publication<rc_parameter_map_s>	_rc_param_map_pub{ORB_ID(rc_parameter_map)};
-	rc_parameter_map_s _rc_param_map{};
-
 	uORB::Publication<uavcan_parameter_request_s> _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)};
 	// enforce ORB_ID(uavcan_parameter_request) constants that map to MAVLINK defines
 	static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ,
@@ -149,6 +150,10 @@ protected:
 		      "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch");
 
 	uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
+#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
+
+	uORB::Publication<rc_parameter_map_s>	_rc_param_map_pub{ORB_ID(rc_parameter_map)};
+	rc_parameter_map_s _rc_param_map{};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	hrt_abstime _param_update_time{0};