subtract timeout from last inertial dead-reck, change fake pos condit…

…ions, save flash
PX4 · Jul 5, 2024 · 9a37cb4 · 9a37cb4
1 parent 6c6660d
commit 9a37cb4
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 10 deletions.
diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu
@@ -7,6 +7,7 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v5x exclude
 # @board px4_fmu-v6x exclude
 #
 

diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp
@@ -75,15 +75,13 @@ void Ekf::controlFakePosFusion()
 					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 					 innov_gate);                                        // innovation gate
 
-		const bool continuing_conditions_passing = !isHorizontalAidingActive()
+		const bool enable_conditions_passing = !isHorizontalAidingActive()
 				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
+				&& (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
 				&& _horizontal_deadreckon_time_exceeded;
 
 		if (_control_status.flags.fake_pos) {
-			if (continuing_conditions_passing) {
+			if (enable_conditions_passing) {
 
 				// always protect against extreme values that could result in a NaN
 				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
@@ -104,7 +102,7 @@ void Ekf::controlFakePosFusion()
 			}
 
 		} else {
-			if (starting_conditions_passing) {
+			if (enable_conditions_passing) {
 				ECL_INFO("start fake position fusion");
 				_control_status.flags.fake_pos = true;
 				resetFakePosFusion();

diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -643,14 +643,16 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 			}
 		}
 
-		_control_status.flags.inertial_dead_reckoning = true;
-
 	} else {
-		_time_last_horizontal_aiding = _time_delayed_us;
+		if (_time_delayed_us > _params.no_aid_timeout_max) {
+			_time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max;
+		}
+
 		_horizontal_deadreckon_time_exceeded = false;
 
-		_control_status.flags.inertial_dead_reckoning = false;
 	}
+
+	_control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning;
 }
 
 void Ekf::updateVerticalDeadReckoningStatus()