ekf2: manual style cleanup after astyle enforcement

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()
 {
 	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()
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
@@ -55,10 +58,8 @@ void Ekf::controlExternalVisionFusion()
 
 		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/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);
 
@@ -365,13 +363,13 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 
 		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
 
-		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_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_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
 
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& _gps_checks_passed
-				&& !is_gps_yaw_data_intermittent
-				&& !_gps_intermittent;
+				&& !data_intermittent;
 
 		if (_control_status.flags.gps_yaw) {
 			if (continuing_conditions_passing) {

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()
 {
 	static constexpr const char *AID_SRC_NAME = "mag";
@@ -134,8 +137,7 @@ void Ekf::controlMagFusion()
 				&& 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,24 @@ void Ekf::controlMagFusion()
 		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.gps_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.gps_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;
+
+		_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));
 
-		// TODO: allow clearing mag_fault if mag_3d is good?
 
 		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)) {
 
@@ -92,6 +90,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

@@ -246,7 +246,7 @@ void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 			      observation_variance,                                                // observation variance
 			      getHagl() - aid_src.observation,                                     // innovation
 			      innovation_variance,                                                 // innovation variance
-			      math::max(_params.range_innov_gate, 1.f));                            // innovation gate
+			      math::max(_params.range_innov_gate, 1.f));                           // innovation gate
 
 	// z special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
@@ -259,11 +259,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.gps_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);
 		}