Merge remote-tracking branch 'px4/main' into pr-ekf2_astyle_follow_up

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -89,8 +89,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 		updateAirspeed(airspeed_sample, _aid_src_airspeed);
 
-		_innov_check_fail_status.flags.reject_airspeed =
-			_aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
+		// TODO: remove this redundant flag
+		_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected;
 
 		const bool continuing_conditions_passing = _control_status.flags.in_air
 				&& _control_status.flags.fixed_wing

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -38,6 +38,9 @@
 
 #include "ekf.h"
 
+// Maximum allowable time interval between pressure altitude measurements (uSec)
+static constexpr uint64_t BARO_MAX_INTERVAL{200'000};
+
 void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "baro";

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -38,6 +38,9 @@
 
 #include "ekf.h"
 
+// Maximum allowable time interval between external vision system measurements (uSec)
+static constexpr uint64_t EV_MAX_INTERVAL{200'000};
+
 void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
@@ -55,10 +58,8 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 
 		const bool starting_conditions_passing = quality_sufficient
 				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
-				&& ((_params.ev_quality_minimum <= 0)
-				    || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-				&& ((_params.ev_quality_minimum <= 0)
-				    || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
+				&& ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum))
+				&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum))
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
 
 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -108,7 +108,7 @@ void Ekf::controlFakePosFusion()
 				resetFakePosFusion();
 
 				if (_control_status.flags.tilt_align) {
-					// The fake position fusion is not started for initial alignement
+					// The fake position fusion is not started for initial alignment
 					ECL_WARN("stopping navigation");
 				}
 			}

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -38,6 +38,9 @@
 
 #include "ekf.h"
 
+// Maximum allowable time interval between GNSS measurements (uSec)
+static constexpr uint64_t GNSS_MAX_INTERVAL{500'000};
+
 void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "GNSS";

src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp

@@ -62,13 +62,13 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
 
 		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
 
-		const bool is_gnss_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push,
-				2 * GNSS_YAW_MAX_INTERVAL);
+		// Maximum allowable time interval between GNSS yaw measurements (uSec)
+		static constexpr uint64_t GNSS_YAW_MAX_INTERVAL{1'500'000};
+		const bool data_intermittent = !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _gps_checks_passed
-				&& !is_gnss_yaw_data_intermittent
-				&& !_gps_intermittent;
+				&& !data_intermittent;
 
 		if (_control_status.flags.gnss_yaw) {
 			if (continuing_conditions_passing) {

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -158,8 +158,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	constexpr float filt_time_const = 10.0f;
-	const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp()))
-					 * 1e-6f, 0.001f, filt_time_const);
+	const int64_t dt_us = int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp());
+	const float dt = math::constrain(fabsf(dt_us) * 1e-6f, 0.001f, filt_time_const);
 	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -55,8 +55,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			      imu_delayed.delta_vel, imu_delayed.delta_vel_dt,
 			      (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest));
 
-	_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
-
 	// check for arrival of new sensor data at the fusion time horizon
 	_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
 

src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp

@@ -83,6 +83,8 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 	// update the states and covariance using sequential fusion
 	for (uint8_t index = 0; index <= 2; index++) {
 		// Calculate Kalman gains and observation jacobians
+		const Vector3f gravity_vector_pred = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f));
+
 		if (index == 0) {
 			// everything was already computed above
 
@@ -91,16 +93,14 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 			sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H);
 
 			// recalculate innovation using the updated state
-			_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f,
-							     -1.f))(index) - measurement(index);
+			_aid_src_gravity.innovation[index] = gravity_vector_pred(index) - measurement(index);
 
 		} else if (index == 2) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
 			sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H);
 
 			// recalculate innovation using the updated state
-			_aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f,
-							     -1.f))(index) - measurement(index);
+			_aid_src_gravity.innovation[index] = gravity_vector_pred(index) - measurement(index);
 		}
 
 		VectorState K = P * H / _aid_src_gravity.innovation_variance[index];

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -41,6 +41,9 @@
 
 #include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
 
+// Maximum allowable time interval between magnetic field measurements (uSec)
+static constexpr uint64_t MAG_MAX_INTERVAL{500'000};
+
 void Ekf::controlMagFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *AID_SRC_NAME = "mag";
@@ -134,8 +137,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 				&& checkMagField(mag_sample.mag)
 				&& (_mag_counter > 3) // wait until we have more than a few samples through the filter
 				&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
-				&& (_state_reset_status.reset_count.quat ==
-				    _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
+				&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat)    // no yaw reset this frame
 				&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
 
 		checkMagHeadingConsistency(mag_sample);
@@ -145,26 +147,23 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
 
 
-		{
-			const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
-			const bool common_conditions_passing = _control_status.flags.mag
-							       && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
-									       || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-							       && !_control_status.flags.mag_fault
-							       && !_control_status.flags.mag_field_disturbed
-							       && !_control_status.flags.ev_yaw
-							       && !_control_status.flags.gnss_yaw;
+		const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding;
 
-			_control_status.flags.mag_3D = common_conditions_passing
-						       && (_params.mag_fusion_type == MagFuseType::AUTO)
-						       && _control_status.flags.mag_aligned_in_flight;
+		const bool common_conditions_passing = _control_status.flags.mag
+						       && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding)
+								       || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+						       && !_control_status.flags.mag_fault
+						       && !_control_status.flags.mag_field_disturbed
+						       && !_control_status.flags.ev_yaw
+						       && !_control_status.flags.gnss_yaw;
 
-			_control_status.flags.mag_hdg = common_conditions_passing
-							&& ((_params.mag_fusion_type == MagFuseType::HEADING)
-							    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
-		}
+		_control_status.flags.mag_3D = common_conditions_passing
+					       && (_params.mag_fusion_type == MagFuseType::AUTO)
+					       && _control_status.flags.mag_aligned_in_flight;
 
-		// TODO: allow clearing mag_fault if mag_3d is good?
+		_control_status.flags.mag_hdg = common_conditions_passing
+						&& ((_params.mag_fusion_type == MagFuseType::HEADING)
+						    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
 
 		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
 			ECL_INFO("starting mag 3D fusion");

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -65,6 +65,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	// update the states and covariance using sequential fusion of the magnetometer components
 	for (uint8_t index = 0; index <= 2; index++) {
 		// Calculate Kalman gains and observation jacobians
+		const Vector3f mag_predicted = _state.quat_nominal.rotateVectorInverse(_state.mag_I) + _state.mag_B;
+
 		if (index == 0) {
 			// everything was already computed
 
@@ -73,8 +75,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 			sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
 
 			// recalculate innovation using the updated state
-			aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(
-							    index);
+			aid_src.innovation[index] = mag_predicted(index) - mag(index);
 
 		} else if (index == 2) {
 			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
@@ -87,8 +88,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 			sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
 
 			// recalculate innovation using the updated state
-			aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(
-							    index);
+			aid_src.innovation[index] = mag_predicted(index) - mag(index);
 		}
 
 		if (aid_src.innovation_variance[index] < R_MAG) {
@@ -174,8 +174,8 @@ bool Ekf::fuseDeclination(float decl_sigma)
 		float decl_pred;
 		float innovation_variance;
 
-		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance,
-				&H);
+		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON,
+				&decl_pred, &innovation_variance, &H);
 
 		const float innovation = wrap_pi(decl_pred - decl_measurement);
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -47,8 +47,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		return;
 	}
 
-	VectorState H;
-
 	// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
 	if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) {
 
@@ -90,6 +88,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const float R_LOS = calcOptFlowMeasVar(flow_sample);
 
 		Vector2f innov_var;
+		VectorState H;
 		sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, FLT_EPSILON, &innov_var, &H);
 
 		// run the innovation consistency check and record result

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -260,11 +260,11 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 
 float Ekf::getRngVar() const
 {
-	return fmaxf(
-		       P(State::pos.idx + 2, State::pos.idx + 2)
-		       + sq(_params.range_noise)
-		       + sq(_params.range_noise_scaler * _range_sensor.getRange()),
-		       0.f);
+	float rng_var = P(State::pos.idx + 2, State::pos.idx + 2)
+			+ sq(_params.range_noise)
+			+ sq(_params.range_noise_scaler * _range_sensor.getRange());
+
+	return math::max(rng_var, 0.f);
 }
 
 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -56,8 +56,8 @@ struct rangeSample {
 	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
 };
 
-static constexpr uint64_t RNG_MAX_INTERVAL =
-	200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
+// Maximum allowable time interval between range finder measurements (uSec)
+static constexpr uint64_t RNG_MAX_INTERVAL{200'000};
 
 class SensorRangeFinder : public Sensor
 {

src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp

@@ -44,8 +44,7 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 				|| _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw;
 
 	// fuse zero innovation at a limited rate if the yaw variance is too large
-	if (!yaw_aiding
-	    && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) {
+	if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, 200'000)) {
 
 		// Use an observation variance larger than usual but small enough
 		// to constrain the yaw variance just below the threshold
@@ -61,7 +60,8 @@ void Ekf::controlZeroInnovationHeadingUpdate()
 		computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW);
 
 		if (!_control_status.flags.tilt_align
-		    || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) {
+		    || ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise))
+		   ) {
 			// The yaw variance is too large, fuse fake measurement
 			fuseYaw(aid_src_status, H_YAW);
 		}

src/modules/ekf2/EKF/common.h

@@ -65,28 +65,6 @@ using math::Utilities::shouldUse321RotationSequence;
 using math::Utilities::sq;
 using math::Utilities::updateYawInRotMat;
 
-// maximum sensor intervals in usec
-static constexpr uint64_t BARO_MAX_INTERVAL     =
-	200e3;  ///< Maximum allowable time interval between pressure altitude measurements (uSec)
-static constexpr uint64_t EV_MAX_INTERVAL       =
-	200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
-static constexpr uint64_t GNSS_MAX_INTERVAL     =
-	500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
-static constexpr uint64_t GNSS_YAW_MAX_INTERVAL =
-	1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
-static constexpr uint64_t MAG_MAX_INTERVAL      =
-	500e3;  ///< Maximum allowable time interval between magnetic field measurements (uSec)
-
-// bad accelerometer detection and mitigation
-static constexpr uint64_t BADACC_PROBATION =
-	10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec)
-static constexpr float BADACC_BIAS_PNOISE =
-	4.9f;  ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2)
-
-// ground effect compensation
-static constexpr uint64_t GNDEFFECT_TIMEOUT =
-	10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec)
-
 enum class PositionFrame : uint8_t {
 	LOCAL_FRAME_NED = 0,
 	LOCAL_FRAME_FRD = 1,
@@ -496,19 +474,18 @@ struct parameters {
 
 union fault_status_u {
 	struct {
-		bool bad_mag_x         : 1; ///< 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error
-		bool bad_mag_y         : 1; ///< 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error
-		bool bad_mag_z         : 1; ///< 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error
-		bool bad_hdg           : 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error
-		bool bad_mag_decl      : 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error
-		bool bad_airspeed      : 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error
-bool bad_sideslip      :
-		1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
-		bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
-		bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
-		bool bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
-		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
-		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
+		bool bad_mag_x         : 1; ///< 0 - fusion of the magnetometer X-axis has encountered a numerical error
+		bool bad_mag_y         : 1; ///< 1 - fusion of the magnetometer Y-axis has encountered a numerical error
+		bool bad_mag_z         : 1; ///< 2 - fusion of the magnetometer Z-axis has encountered a numerical error
+		bool bad_hdg           : 1; ///< 3 - fusion of the heading angle has encountered a numerical error
+		bool bad_mag_decl      : 1; ///< 4 - fusion of the magnetic declination has encountered a numerical error
+		bool bad_airspeed      : 1; ///< 5 - fusion of the airspeed has encountered a numerical error
+		bool bad_sideslip      : 1; ///< 6 - fusion of the synthetic sideslip constraint encountered a numerical error
+		bool bad_optflow_X     : 1; ///< 7 - fusion of the optical flow X axis has encountered a numerical error
+		bool bad_optflow_Y     : 1; ///< 8 - fusion of the optical flow Y axis has encountered a numerical error
+		bool bad_acc_bias      : 1; ///< 9 - bad delta velocity bias estimates have been detected
+		bool bad_acc_vertical  : 1; ///< 10 - bad vertical accelerometer data has been detected
+		bool bad_acc_clipping  : 1; ///< 11 - delta velocity data contains clipping (asymmetric railing)
 	} flags;
 	uint32_t value;
 };
@@ -516,19 +493,19 @@ bool bad_sideslip      :
 // define structure used to communicate innovation test failures
 union innovation_fault_status_u {
 	struct {
-		bool reject_hor_vel   : 1; ///< 0 - true if horizontal velocity observations have been rejected
-		bool reject_ver_vel   : 1; ///< 1 - true if vertical velocity observations have been rejected
-		bool reject_hor_pos   : 1; ///< 2 - true if horizontal position observations have been rejected
-		bool reject_ver_pos   : 1; ///< 3 - true if true if vertical position observations have been rejected
-		bool reject_mag_x     : 1; ///< 4 - true if the X magnetometer observation has been rejected
-		bool reject_mag_y     : 1; ///< 5 - true if the Y magnetometer observation has been rejected
-		bool reject_mag_z     : 1; ///< 6 - true if the Z magnetometer observation has been rejected
-		bool reject_yaw       : 1; ///< 7 - true if the yaw observation has been rejected
-		bool reject_airspeed  : 1; ///< 8 - true if the airspeed observation has been rejected
-		bool reject_sideslip  : 1; ///< 9 - true if the synthetic sideslip observation has been rejected
+		bool reject_hor_vel   : 1; ///< 0 - horizontal velocity observations have been rejected
+		bool reject_ver_vel   : 1; ///< 1 - vertical velocity observations have been rejected
+		bool reject_hor_pos   : 1; ///< 2 - horizontal position observations have been rejected
+		bool reject_ver_pos   : 1; ///< 3 - vertical position observations have been rejected
+		bool reject_mag_x     : 1; ///< 4 - X magnetometer observation has been rejected
+		bool reject_mag_y     : 1; ///< 5 - Y magnetometer observation has been rejected
+		bool reject_mag_z     : 1; ///< 6 - Z magnetometer observation has been rejected
+		bool reject_yaw       : 1; ///< 7 - yaw observation has been rejected
+		bool reject_airspeed  : 1; ///< 8 - airspeed observation has been rejected
+		bool reject_sideslip  : 1; ///< 9 - synthetic sideslip observation has been rejected
 		bool reject_hagl      : 1; ///< 10 - unused
-		bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected
-		bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected
+		bool reject_optflow_X : 1; ///< 11 - X optical flow observation has been rejected
+		bool reject_optflow_Y : 1; ///< 12 - Y optical flow observation has been rejected
 	} flags;
 	uint16_t value;
 };
@@ -536,17 +513,17 @@ union innovation_fault_status_u {
 // publish the status of various GPS quality checks
 union gps_check_fail_status_u {
 	struct {
-		uint16_t fix    : 1; ///< 0 - true if the fix type is insufficient (no 3D solution)
-		uint16_t nsats  : 1; ///< 1 - true if number of satellites used is insufficient
-		uint16_t pdop   : 1; ///< 2 - true if position dilution of precision is insufficient
-		uint16_t hacc   : 1; ///< 3 - true if reported horizontal accuracy is insufficient
-		uint16_t vacc   : 1; ///< 4 - true if reported vertical accuracy is insufficient
-		uint16_t sacc   : 1; ///< 5 - true if reported speed accuracy is insufficient
-		uint16_t hdrift : 1; ///< 6 - true if horizontal drift is excessive (can only be used when stationary on ground)
-		uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground)
-		uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
-		uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
-		uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed
+		uint16_t fix    : 1; ///< 0 - fix type is insufficient (no 3D solution)
+		uint16_t nsats  : 1; ///< 1 - number of satellites used is insufficient
+		uint16_t pdop   : 1; ///< 2 - position dilution of precision is insufficient
+		uint16_t hacc   : 1; ///< 3 - reported horizontal accuracy is insufficient
+		uint16_t vacc   : 1; ///< 4 - reported vertical accuracy is insufficient
+		uint16_t sacc   : 1; ///< 5 - reported speed accuracy is insufficient
+		uint16_t hdrift : 1; ///< 6 - horizontal drift is excessive (can only be used when stationary on ground)
+		uint16_t vdrift : 1; ///< 7 - vertical drift is excessive (can only be used when stationary on ground)
+		uint16_t hspeed : 1; ///< 8 - horizontal speed is excessive (can only be used when stationary on ground)
+		uint16_t vspeed : 1; ///< 9 - vertical speed error is excessive
+		uint16_t spoofed: 1; ///< 10 - GNSS data reports spoofing
 	} flags;
 	uint16_t value;
 };
@@ -554,60 +531,47 @@ union gps_check_fail_status_u {
 // bitmask containing filter control status
 union filter_control_status_u {
 	struct {
-		uint64_t tilt_align              : 1; ///< 0 - true if the filter tilt alignment is complete
-		uint64_t yaw_align               : 1; ///< 1 - true if the filter yaw alignment is complete
-		uint64_t gps                     : 1; ///< 2 - true if GPS measurement fusion is intended
-		uint64_t opt_flow                : 1; ///< 3 - true if optical flow measurements fusion is intended
-		uint64_t mag_hdg                 : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
-		uint64_t mag_3D                  : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is intended
-		uint64_t mag_dec                 : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
-		uint64_t in_air                  : 1; ///< 7 - true when the vehicle is airborne
-		uint64_t wind                    : 1; ///< 8 - true when wind velocity is being estimated
-		uint64_t baro_hgt                : 1; ///< 9 - true when baro height is being fused as a primary height reference
-uint64_t rng_hgt                 :
-		1; ///< 10 - true when range finder height is being fused as a primary height reference
-		uint64_t gps_hgt                 : 1; ///< 11 - true when GPS height is being fused as a primary height reference
-		uint64_t ev_pos                  : 1; ///< 12 - true when local position data fusion from external vision is intended
-		uint64_t ev_yaw                  : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
-		uint64_t ev_hgt                  : 1; ///< 14 - true when height data from external vision measurements is being fused
-		uint64_t fuse_beta               : 1; ///< 15 - true when synthetic sideslip measurements are being fused
-		uint64_t mag_field_disturbed     : 1; ///< 16 - true when the mag field does not match the expected strength
-		uint64_t fixed_wing              : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
-uint64_t mag_fault               :
-		1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
-		uint64_t fuse_aspd               : 1; ///< 19 - true when airspeed measurements are being fused
-uint64_t gnd_effect              :
-		1; ///< 20 - true when protection from ground effect induced static pressure rise is active
-uint64_t rng_stuck               :
-		1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
-uint64_t gnss_yaw                 :
-		1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
-		uint64_t mag_aligned_in_flight   : 1; ///< 23 - true when the in-flight mag field alignment has been completed
-uint64_t ev_vel                  :
-		1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
-uint64_t synthetic_mag_z         :
-		1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
-		uint64_t vehicle_at_rest         : 1; ///< 26 - true when the vehicle is at rest
-uint64_t gnss_yaw_fault           :
-		1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint64_t rng_fault               :
-		1; ///< 28 - true when the range finder has been declared faulty and is no longer being used
-uint64_t inertial_dead_reckoning :
-		1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
-		uint64_t wind_dead_reckoning     : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements
-		uint64_t rng_kin_consistent      : 1; ///< 31 - true when the range finder kinematic consistency check is passing
-		uint64_t fake_pos                : 1; ///< 32 - true when fake position measurements are being fused
-		uint64_t fake_hgt                : 1; ///< 33 - true when fake height measurements are being fused
-		uint64_t gravity_vector          : 1; ///< 34 - true when gravity vector measurements are being fused
-uint64_t mag                     :
-		1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended
-uint64_t ev_yaw_fault            :
-		1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used
-uint64_t mag_heading_consistent  :
-		1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter
-		uint64_t aux_gpos                : 1; ///< 38 - true if auxiliary global position measurement fusion is intended
-		uint64_t rng_terrain             : 1; ///< 39 - true if we are fusing range finder data for terrain
-		uint64_t opt_flow_terrain        : 1; ///< 40 - true if we are fusing flow data for terrain
+		uint64_t tilt_align              : 1; ///< 0 - filter tilt alignment is complete
+		uint64_t yaw_align               : 1; ///< 1 - filter yaw alignment is complete
+		uint64_t gps                     : 1; ///< 2 - GPS measurement fusion is intended
+		uint64_t opt_flow                : 1; ///< 3 - optical flow measurements fusion is intended
+		uint64_t mag_hdg                 : 1; ///< 4 - a simple magnetic yaw heading fusion is intended
+		uint64_t mag_3D                  : 1; ///< 5 - 3-axis magnetometer measurement fusion is intended
+		uint64_t mag_dec                 : 1; ///< 6 - synthetic magnetic declination measurements fusion is intended
+		uint64_t in_air                  : 1; ///< 7 - the vehicle is airborne
+		uint64_t wind                    : 1; ///< 8 - wind velocity is being estimated
+		uint64_t baro_hgt                : 1; ///< 9 - baro height is being fused as a primary height reference
+		uint64_t rng_hgt                 : 1; ///< 10 - range finder height is being fused as a primary height ref
+		uint64_t gps_hgt                 : 1; ///< 11 - GPS height is being fused as a primary height reference
+		uint64_t ev_pos                  : 1; ///< 12 - local position data fusion from external vision is intended
+		uint64_t ev_yaw                  : 1; ///< 13 - yaw data from external vision measurements fusion is intended
+		uint64_t ev_hgt                  : 1; ///< 14 - height data from external vision measurements is being fused
+		uint64_t fuse_beta               : 1; ///< 15 - synthetic sideslip measurements are being fused
+		uint64_t mag_field_disturbed     : 1; ///< 16 - the mag field does not match the expected strength
+		uint64_t fixed_wing              : 1; ///< 17 - the vehicle is operating as a fixed wing vehicle
+		uint64_t mag_fault               : 1; ///< 18 - the mag has been declared faulty and is no longer being used
+		uint64_t fuse_aspd               : 1; ///< 19 - airspeed measurements are being fused
+		uint64_t gnd_effect              : 1; ///< 20 - protection from ground effect induced static pressure rise is active
+		uint64_t rng_stuck               : 1; ///< 21 - rng data wasn't ready and new values haven't changed enough
+		uint64_t gnss_yaw                : 1; ///< 22 - yaw (not ground course) data fusion from a GPS receiver is intended
+		uint64_t mag_aligned_in_flight   : 1; ///< 23 - the in-flight mag field alignment has been completed
+		uint64_t ev_vel                  : 1; ///< 24 - local frame velocity data fusion from vision measurements is intended
+		uint64_t synthetic_mag_z         : 1; ///< 25 - using a synthesized measurement for the magnetometer Z component
+		uint64_t vehicle_at_rest         : 1; ///< 26 - the vehicle is at rest
+		uint64_t gnss_yaw_fault          : 1; ///< 27 - the GNSS heading has been declared faulty and is no longer being used
+		uint64_t rng_fault               : 1; ///< 28 - the range finder has been declared faulty and is no longer being used
+		uint64_t inertial_dead_reckoning : 1; ///< 29 - longer fusing measurements that constrain horizontal velocity drift
+		uint64_t wind_dead_reckoning     : 1; ///< 30 - navigation reliant on wind relative measurements
+		uint64_t rng_kin_consistent      : 1; ///< 31 - the range finder kinematic consistency check is passing
+		uint64_t fake_pos                : 1; ///< 32 - fake position measurements are being fused
+		uint64_t fake_hgt                : 1; ///< 33 - fake height measurements are being fused
+		uint64_t gravity_vector          : 1; ///< 34 - gravity vector measurements are being fused
+		uint64_t mag                     : 1; ///< 35 - 3-axis magnetometer measurement fusion (mag states only) is intended
+		uint64_t ev_yaw_fault            : 1; ///< 36 - the EV heading has been declared faulty and is no longer being used
+		uint64_t mag_heading_consistent  : 1; ///< 37 - the heading obtained mag is declared consistent with the filter
+		uint64_t aux_gpos                : 1; ///< 38 - auxiliary global position measurement fusion is intended
+		uint64_t rng_terrain             : 1; ///< 39 - fusing range finder data for terrain
+		uint64_t opt_flow_terrain        : 1; ///< 40 - fusing flow data for terrain
 
 	} flags;
 	uint64_t value;
@@ -616,21 +580,18 @@ uint64_t mag_heading_consistent  :
 // Mavlink bitmask containing state of estimator solution
 union ekf_solution_status_u {
 	struct {
-		uint16_t attitude           : 1; ///< 0 - True if the attitude estimate is good
-		uint16_t velocity_horiz     : 1; ///< 1 - True if the horizontal velocity estimate is good
-		uint16_t velocity_vert      : 1; ///< 2 - True if the vertical velocity estimate is good
-		uint16_t pos_horiz_rel      : 1; ///< 3 - True if the horizontal position (relative) estimate is good
-		uint16_t pos_horiz_abs      : 1; ///< 4 - True if the horizontal position (absolute) estimate is good
-		uint16_t pos_vert_abs       : 1; ///< 5 - True if the vertical position (absolute) estimate is good
-		uint16_t pos_vert_agl       : 1; ///< 6 - True if the vertical position (above ground) estimate is good
-uint16_t const_pos_mode     :
-		1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
-uint16_t pred_pos_horiz_rel :
-		1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
-uint16_t pred_pos_horiz_abs :
-		1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate
-		uint16_t gps_glitch         : 1; ///< 10 - True if the EKF has detected a GPS glitch
-		uint16_t accel_error        : 1; ///< 11 - True if the EKF has detected bad accelerometer data
+		uint16_t attitude           : 1; ///< 0 - attitude estimate is good
+		uint16_t velocity_horiz     : 1; ///< 1 - horizontal velocity estimate is good
+		uint16_t velocity_vert      : 1; ///< 2 - vertical velocity estimate is good
+		uint16_t pos_horiz_rel      : 1; ///< 3 - horizontal position (relative) estimate is good
+		uint16_t pos_horiz_abs      : 1; ///< 4 - horizontal position (absolute) estimate is good
+		uint16_t pos_vert_abs       : 1; ///< 5 - vertical position (absolute) estimate is good
+		uint16_t pos_vert_agl       : 1; ///< 6 - vertical position (above ground) estimate is good
+		uint16_t const_pos_mode     : 1; ///< 7 - in a constant position mode and is not using external measurements
+		uint16_t pred_pos_horiz_rel : 1; ///< 8 - sufficient data for a (relative) position estimate
+		uint16_t pred_pos_horiz_abs : 1; ///< 9 - sufficient data for a (absolute) position estimate
+		uint16_t gps_glitch         : 1; ///< 10 - detected a GPS glitch
+		uint16_t accel_error        : 1; ///< 11 - EKF has detected bad accelerometer data
 	} flags;
 	uint16_t value;
 };
@@ -638,30 +599,24 @@ uint16_t pred_pos_horiz_abs :
 // define structure used to communicate information events
 union information_event_status_u {
 	struct {
-		bool gps_checks_passed          : 1; ///< 0 - true when gps quality checks are passing passed
-		bool reset_vel_to_gps           : 1; ///< 1 - true when the velocity states are reset to the gps measurement
-		bool reset_vel_to_flow          : 1; ///< 2 - true when the velocity states are reset using the optical flow measurement
-		bool reset_vel_to_vision        : 1; ///< 3 - true when the velocity states are reset to the vision system measurement
-		bool reset_vel_to_zero          : 1; ///< 4  - true when the velocity states are reset to zero
-		bool reset_pos_to_last_known    : 1; ///< 5 - true when the position states are reset to the last known position
-		bool reset_pos_to_gps           : 1; ///< 6 - true when the position states are reset to the gps measurement
-		bool reset_pos_to_vision        : 1; ///< 7 - true when the position states are reset to the vision system measurement
-bool starting_gps_fusion        :
-		1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates
-bool starting_vision_pos_fusion :
-		1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates
-bool starting_vision_vel_fusion :
-		1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
-bool starting_vision_yaw_fusion :
-		1; ///< 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
-bool yaw_aligned_to_imu_gps     :
-		1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
-		bool reset_hgt_to_baro          : 1; ///< 13 - true when the vertical position state is reset to the baro measurement
-		bool reset_hgt_to_gps           : 1; ///< 14 - true when the vertical position state is reset to the gps measurement
-		bool reset_hgt_to_rng           : 1; ///< 15 - true when the vertical position state is reset to the rng measurement
-		bool reset_hgt_to_ev            : 1; ///< 16 - true when the vertical position state is reset to the ev measurement
-bool reset_pos_to_ext_obs       :
-		1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning
+		bool gps_checks_passed          : 1; ///< 0 - gps quality checks are passing passed
+		bool reset_vel_to_gps           : 1; ///< 1 - the velocity states are reset to the gps measurement
+		bool reset_vel_to_flow          : 1; ///< 2 - the velocity states are reset using the optical flow measurement
+		bool reset_vel_to_vision        : 1; ///< 3 - the velocity states are reset to the vision system measurement
+		bool reset_vel_to_zero          : 1; ///< 4 - the velocity states are reset to zero
+		bool reset_pos_to_last_known    : 1; ///< 5 - the position states are reset to the last known position
+		bool reset_pos_to_gps           : 1; ///< 6 - the position states are reset to the gps measurement
+		bool reset_pos_to_vision        : 1; ///< 7 - the position states are reset to the vision system measurement
+		bool starting_gps_fusion        : 1; ///< 8 - started using gps measurements to correct the state estimates
+		bool starting_vision_pos_fusion : 1; ///< 9 - started using vision position measurements to correct the state estimates
+		bool starting_vision_vel_fusion : 1; ///< 10 - started using vision velocity measurements to correct the state estimates
+		bool starting_vision_yaw_fusion : 1; ///< 11 - started using vision yaw measurements to correct the state estimates
+		bool yaw_aligned_to_imu_gps     : 1; ///< 12 - the filter resets the yaw to an estimate derived from IMU and GPS data
+		bool reset_hgt_to_baro          : 1; ///< 13 - the vertical position state is reset to the baro measurement
+		bool reset_hgt_to_gps           : 1; ///< 14 - the vertical position state is reset to the gps measurement
+		bool reset_hgt_to_rng           : 1; ///< 15 - the vertical position state is reset to the rng measurement
+		bool reset_hgt_to_ev            : 1; ///< 16 - the vertical position state is reset to the ev measurement
+		bool reset_pos_to_ext_obs       : 1; ///< 17 - position was reset to an external observation while deadreckoning
 	} flags;
 	uint32_t value;
 };

src/modules/ekf2/EKF/covariance.cpp

@@ -125,6 +125,7 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 	for (unsigned i = 0; i < 3; i++) {
 		if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) {
 			// Increase accelerometer process noise if bad accel data is detected
+			static constexpr float BADACC_BIAS_PNOISE{4.9f}; ///< IMU acceleration noise when accel data is declared bad (m/sec**2)
 			accel_var(i) = sq(BADACC_BIAS_PNOISE);
 
 		} else {
@@ -202,8 +203,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 
 	if (_control_status.flags.wind) {
 		// wind vel: add process noise
-		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f,
-					    1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
+		float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f)
+					    * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
 		float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt;
 
 		for (unsigned index = 0; index < State::wind_vel.dof; index++) {
@@ -222,8 +223,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)
 		float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise);
 
 		// process noise due to terrain gradient
-		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(
-						 1)));
+		terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient)
+					 * (sq(_state.vel(0)) + sq(_state.vel(1)));
 		P(State::terrain.idx, State::terrain.idx) += terrain_process_noise;
 	}
 

src/modules/ekf2/EKF/ekf.h

@@ -659,8 +659,6 @@ private:
 	bool _gps_checks_passed{false};		///> true when all active GPS checks have passed
 
 	gps_check_fail_status_u _gps_check_fail_status{};
-	// height sensor status
-	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
 
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -72,8 +72,8 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	return _NED_origin_initialised;
 }
 
-bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
-			     const float epv)
+bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude,
+			     const float eph, const float epv)
 {
 	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
 	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
@@ -536,14 +536,21 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	ekf_solution_status_u soln_status{};
 	// TODO: Is this accurate enough?
 	soln_status.flags.attitude = attitude_valid();
-	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta
-					    && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
+
+	soln_status.flags.velocity_horiz = (isHorizontalAidingActive()
+					    || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
+
 	soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt
-					   || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
+					   || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt)
+					  && (_fault_status.value == 0);
+
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos
-					   || _control_status.flags.opt_flow) && (_fault_status.value == 0);
+					   || _control_status.flags.opt_flow)
+					  && (_fault_status.value == 0);
+
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos)
 					  && (_fault_status.value == 0);
+
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
 #if defined(CONFIG_EKF2_TERRAIN)
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
@@ -580,36 +587,37 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 void Ekf::fuse(const VectorState &K, float innovation)
 {
 	// quat_nominal
-	Quatf delta_quat(matrix::AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx,
-					    0) * (-1.f * innovation)));
+	Quatf delta_quat(AxisAnglef(K.slice<State::quat_nominal.dof, 1>(State::quat_nominal.idx, 0)
+				    * (-1.f * innovation)));
 	_state.quat_nominal = delta_quat * _state.quat_nominal;
 	_state.quat_nominal.normalize();
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// vel
-	_state.vel = matrix::constrain(_state.vel - K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f);
+	_state.vel -= K.slice<State::vel.dof, 1>(State::vel.idx, 0) * innovation;
+	_state.vel = matrix::constrain(_state.vel, -1.e3f, 1.e3f);
 
 	// pos
-	_state.pos = matrix::constrain(_state.pos - K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f);
+	_state.pos -= K.slice<State::pos.dof, 1>(State::pos.idx, 0) * innovation;
+	_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
 
 	// gyro_bias
-	_state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx,
-					     0) * innovation,
-					     -getGyroBiasLimit(), getGyroBiasLimit());
+	_state.gyro_bias -= K.slice<State::gyro_bias.dof, 1>(State::gyro_bias.idx, 0) * innovation;
+	_state.gyro_bias = matrix::constrain(_state.gyro_bias, -getGyroBiasLimit(), getGyroBiasLimit());
 
 	// accel_bias
-	_state.accel_bias = matrix::constrain(_state.accel_bias - K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx,
-					      0) * innovation,
-					      -getAccelBiasLimit(), getAccelBiasLimit());
+	_state.accel_bias -= K.slice<State::accel_bias.dof, 1>(State::accel_bias.idx, 0) * innovation;
+	_state.accel_bias = matrix::constrain(_state.accel_bias, -getAccelBiasLimit(), getAccelBiasLimit());
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 
 	// mag_I, mag_B
 	if (_control_status.flags.mag) {
-		_state.mag_I = matrix::constrain(_state.mag_I - K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation, -1.f,
-						 1.f);
-		_state.mag_B = matrix::constrain(_state.mag_B - K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation,
-						 -getMagBiasLimit(), getMagBiasLimit());
+		_state.mag_I -= K.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) * innovation;
+		_state.mag_I = matrix::constrain(_state.mag_I, -1.f, 1.f);
+
+		_state.mag_B -= K.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) * innovation;
+		_state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit());
 	}
 
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -618,8 +626,8 @@ void Ekf::fuse(const VectorState &K, float innovation)
 
 	// wind_vel
 	if (_control_status.flags.wind) {
-		_state.wind_vel = matrix::constrain(_state.wind_vel - K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx,
-						    0) * innovation, -1.e2f, 1.e2f);
+		_state.wind_vel -= K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) * innovation;
+		_state.wind_vel = matrix::constrain(_state.wind_vel, -1.e2f, 1.e2f);
 	}
 
 #endif // CONFIG_EKF2_WIND
@@ -787,6 +795,8 @@ void Ekf::updateGroundEffect()
 #endif // CONFIG_EKF2_TERRAIN
 			if (_control_status.flags.gnd_effect) {
 				// Turn off ground effect compensation if it times out
+				static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6;
+
 				if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 					_control_status.flags.gnd_effect = false;
 				}

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -152,8 +152,8 @@ void EstimatorInterface::setMagData(const magSample &mag_sample)
 		_time_last_mag_buffer_push = _time_latest_us;
 
 	} else {
-		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_MAGNETOMETER
@@ -199,8 +199,8 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 #endif // CONFIG_EKF2_GNSS_YAW
 
 	} else {
-		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_GNSS
@@ -238,8 +238,8 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 		_time_last_baro_buffer_push = _time_latest_us;
 
 	} else {
-		ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_BAROMETER
@@ -276,8 +276,8 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 		_airspeed_buffer->push(airspeed_sample_new);
 
 	} else {
-		ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_AIRSPEED
@@ -315,8 +315,8 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 		_time_last_range_buffer_push = _time_latest_us;
 
 	} else {
-		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -353,8 +353,8 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 		_flow_buffer->push(optflow_sample_new);
 
 	} else {
-		ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -393,8 +393,8 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 		_time_last_ext_vision_buffer_push = _time_latest_us;
 
 	} else {
-		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_EXTERNAL_VISION
@@ -431,8 +431,8 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 		_auxvel_buffer->push(auxvel_sample_new);
 
 	} else {
-		ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us,
-			 _min_obs_interval_us);
+		ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d",
+			 time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 #endif // CONFIG_EKF2_AUXVEL
@@ -467,8 +467,8 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 		_system_flag_buffer->push(system_flags_new);
 
 	} else {
-		ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us,
-			  _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
+		ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d",
+			  time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
 

src/modules/ekf2/EKF/height_control.cpp

@@ -269,6 +269,8 @@ void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
 
 	// declare a bad vertical acceleration measurement and make the declaration persist
 	// for a minimum of BADACC_PROBATION seconds
+	static constexpr uint64_t BADACC_PROBATION = 10e6;
+
 	if (_fault_status.flags.bad_acc_vertical) {
 		_fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION);
 

src/modules/ekf2/EKF/output_predictor/output_predictor.cpp

@@ -261,8 +261,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector
 }
 
 void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us,
-		const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias,
-		const matrix::Vector3f &accel_bias)
+		const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state,
+		const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias)
 {
 	// calculate an average filter update time
 	if (_time_last_correct_states_us != 0) {

src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp

@@ -301,8 +301,8 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
 	// Use fixed values for delta angle process noise variances
 	const float d_ang_var = sq(_gyro_noise * delta_ang_dt);
 
-	_ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx,
-				  dvy), d_vel_var, daz, d_ang_var);
+	_ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P,
+				  Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var);
 
 	// covariance matrix is symmetrical, so copy upper half to lower half
 	_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);

src/modules/ekf2/EKF2.cpp

@@ -1889,36 +1889,36 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		estimator_status_flags_s status_flags{};
 		status_flags.timestamp_sample = _ekf.time_delayed_us();
 
-		status_flags.control_status_changes   = _filter_control_status_changes;
-		status_flags.cs_tilt_align            = _ekf.control_status_flags().tilt_align;
-		status_flags.cs_yaw_align             = _ekf.control_status_flags().yaw_align;
-		status_flags.cs_gps                   = _ekf.control_status_flags().gps;
-		status_flags.cs_opt_flow              = _ekf.control_status_flags().opt_flow;
-		status_flags.cs_mag_hdg               = _ekf.control_status_flags().mag_hdg;
-		status_flags.cs_mag_3d                = _ekf.control_status_flags().mag_3D;
-		status_flags.cs_mag_dec               = _ekf.control_status_flags().mag_dec;
-		status_flags.cs_in_air                = _ekf.control_status_flags().in_air;
-		status_flags.cs_wind                  = _ekf.control_status_flags().wind;
-		status_flags.cs_baro_hgt              = _ekf.control_status_flags().baro_hgt;
-		status_flags.cs_rng_hgt               = _ekf.control_status_flags().rng_hgt;
-		status_flags.cs_gps_hgt               = _ekf.control_status_flags().gps_hgt;
-		status_flags.cs_ev_pos                = _ekf.control_status_flags().ev_pos;
-		status_flags.cs_ev_yaw                = _ekf.control_status_flags().ev_yaw;
-		status_flags.cs_ev_hgt                = _ekf.control_status_flags().ev_hgt;
-		status_flags.cs_fuse_beta             = _ekf.control_status_flags().fuse_beta;
-		status_flags.cs_mag_field_disturbed   = _ekf.control_status_flags().mag_field_disturbed;
-		status_flags.cs_fixed_wing            = _ekf.control_status_flags().fixed_wing;
-		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
-		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
-		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
-		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
-		status_flags.cs_gnss_yaw               = _ekf.control_status_flags().gnss_yaw;
-		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
-		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
-		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
-		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
-		status_flags.cs_gnss_yaw_fault         = _ekf.control_status_flags().gnss_yaw_fault;
-		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
+		status_flags.control_status_changes     = _filter_control_status_changes;
+		status_flags.cs_tilt_align              = _ekf.control_status_flags().tilt_align;
+		status_flags.cs_yaw_align               = _ekf.control_status_flags().yaw_align;
+		status_flags.cs_gps                     = _ekf.control_status_flags().gps;
+		status_flags.cs_opt_flow                = _ekf.control_status_flags().opt_flow;
+		status_flags.cs_mag_hdg                 = _ekf.control_status_flags().mag_hdg;
+		status_flags.cs_mag_3d                  = _ekf.control_status_flags().mag_3D;
+		status_flags.cs_mag_dec                 = _ekf.control_status_flags().mag_dec;
+		status_flags.cs_in_air                  = _ekf.control_status_flags().in_air;
+		status_flags.cs_wind                    = _ekf.control_status_flags().wind;
+		status_flags.cs_baro_hgt                = _ekf.control_status_flags().baro_hgt;
+		status_flags.cs_rng_hgt                 = _ekf.control_status_flags().rng_hgt;
+		status_flags.cs_gps_hgt                 = _ekf.control_status_flags().gps_hgt;
+		status_flags.cs_ev_pos                  = _ekf.control_status_flags().ev_pos;
+		status_flags.cs_ev_yaw                  = _ekf.control_status_flags().ev_yaw;
+		status_flags.cs_ev_hgt                  = _ekf.control_status_flags().ev_hgt;
+		status_flags.cs_fuse_beta               = _ekf.control_status_flags().fuse_beta;
+		status_flags.cs_mag_field_disturbed     = _ekf.control_status_flags().mag_field_disturbed;
+		status_flags.cs_fixed_wing              = _ekf.control_status_flags().fixed_wing;
+		status_flags.cs_mag_fault               = _ekf.control_status_flags().mag_fault;
+		status_flags.cs_fuse_aspd               = _ekf.control_status_flags().fuse_aspd;
+		status_flags.cs_gnd_effect              = _ekf.control_status_flags().gnd_effect;
+		status_flags.cs_rng_stuck               = _ekf.control_status_flags().rng_stuck;
+		status_flags.cs_gnss_yaw                = _ekf.control_status_flags().gnss_yaw;
+		status_flags.cs_mag_aligned_in_flight   = _ekf.control_status_flags().mag_aligned_in_flight;
+		status_flags.cs_ev_vel                  = _ekf.control_status_flags().ev_vel;
+		status_flags.cs_synthetic_mag_z         = _ekf.control_status_flags().synthetic_mag_z;
+		status_flags.cs_vehicle_at_rest         = _ekf.control_status_flags().vehicle_at_rest;
+		status_flags.cs_gnss_yaw_fault          = _ekf.control_status_flags().gnss_yaw_fault;
+		status_flags.cs_rng_fault               = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
@@ -1929,8 +1929,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
 		status_flags.cs_mag_heading_consistent  = _ekf.control_status_flags().mag_heading_consistent;
 		status_flags.cs_aux_gpos                = _ekf.control_status_flags().aux_gpos;
-		status_flags.cs_rng_terrain    = _ekf.control_status_flags().rng_terrain;
-		status_flags.cs_opt_flow_terrain    = _ekf.control_status_flags().opt_flow_terrain;
+		status_flags.cs_rng_terrain             = _ekf.control_status_flags().rng_terrain;
+		status_flags.cs_opt_flow_terrain        = _ekf.control_status_flags().opt_flow_terrain;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;