navigator: use hysteresis to delay neutral gimbal command

src/modules/navigator/land.cpp

@@ -72,7 +72,7 @@ Land::on_activation()
 	// reset cruising speed to default
 	_navigator->reset_cruising_speed();
 
-	_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
+	_navigator->gimbal_neutral_delayed();
 }
 
 void

src/modules/navigator/mission_base.cpp

@@ -182,7 +182,7 @@ MissionBase::on_inactivation()
 	_navigator->stop_capturing_images();
 
 	if (!_navigator->get_land_detected()->landed) {
-		_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
+		_navigator->gimbal_neutral_delayed();
 	}
 
 	if (_navigator->get_precland()->is_activated()) {
@@ -702,7 +702,7 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s
 			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
 			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
 
-			_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
+			_navigator->gimbal_neutral_delayed();
 
 		} else {
 

src/modules/navigator/navigator.h

@@ -60,6 +60,7 @@
 #if CONFIG_NAVIGATOR_ADSB
 #include <lib/adsb/AdsbConflict.h>
 #endif // CONFIG_NAVIGATOR_ADSB
+#include <lib/hysteresis/hysteresis.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/events.h>
 #include <px4_platform_common/module.h>
@@ -281,13 +282,7 @@ public:
 	void acquire_gimbal_control();
 	void release_gimbal_control();
 	void set_gimbal_neutral();
-
-	/* Set gimbal to neutral position (level with horizon) to reduce risk of damage on landing.
-	The commands are executed after time delay. */
-	void neutralize_gimbal_if_control_activated();
-	/* Accepts a new timestamp only if the current timestamp is UINT64_MAX, preventing the
-	timer from resetting during an ongoing neutral command. */
-	void activate_set_gimbal_neutral_timer(const hrt_abstime timestamp);
+	void gimbal_neutral_delayed() { _gimbal_neutral_hysteresis.set_state_and_update(true, hrt_absolute_time()); }; // Acquires and releases control
 
 	void preproject_stop_point(double &lat, double &lon);
 
@@ -397,9 +392,7 @@ private:
 
 	bool _is_capturing_images{false}; // keep track if we need to stop capturing images
 
-
-	// timer to trigger a delayed set gimbal neutral command
-	hrt_abstime _gimbal_neutral_activation_time{UINT64_MAX};
+	systemlib::Hysteresis _gimbal_neutral_hysteresis{false}; ///< Delay to avoid race condition with previous mode still wanting gimbal control e.g. slow mode flight task
 
 	// update subscriptions
 	void params_update();

src/modules/navigator/navigator_main.cpp

@@ -114,6 +114,8 @@ Navigator::Navigator() :
 	_distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF;
 	_distance_sensor_mode_change_request_pub.update();
 
+	_gimbal_neutral_hysteresis.set_hysteresis_time_from(false, 250_ms);
+
 	reset_triplets();
 }
 
@@ -924,7 +926,16 @@ void Navigator::run()
 			publish_mission_result();
 		}
 
-		neutralize_gimbal_if_control_activated();
+		// Set gimbal neutral if requested and delay is over
+		const hrt_abstime now = hrt_absolute_time();
+		_gimbal_neutral_hysteresis.update(now);
+
+		if (_gimbal_neutral_hysteresis.get_state()) {
+			acquire_gimbal_control();
+			set_gimbal_neutral();
+			release_gimbal_control();
+			_gimbal_neutral_hysteresis.set_state_and_update(false, now); // instantly resets from true to false
+		}
 
 		publish_navigator_status();
 
@@ -1651,27 +1662,6 @@ void Navigator::set_gimbal_neutral()
 	publish_vehicle_command(vehicle_command);
 }
 
-void Navigator::activate_set_gimbal_neutral_timer(const hrt_abstime timestamp)
-{
-	if (_gimbal_neutral_activation_time == UINT64_MAX) {
-		_gimbal_neutral_activation_time = timestamp;
-	}
-}
-
-void Navigator::neutralize_gimbal_if_control_activated()
-{
-	const hrt_abstime now{hrt_absolute_time()};
-
-	// The time delay must be sufficiently long to allow flight tasks to complete its
-	// destruction and release gimbal control before the navigator takes control of the gimbal.
-	if (_gimbal_neutral_activation_time != UINT64_MAX && now > _gimbal_neutral_activation_time + 250_ms) {
-		acquire_gimbal_control();
-		set_gimbal_neutral();
-		release_gimbal_control();
-		_gimbal_neutral_activation_time = UINT64_MAX;
-	}
-}
-
 void Navigator::sendWarningDescentStoppedDueToTerrain()
 {
 	mavlink_log_critical(&_mavlink_log_pub, "Terrain collision risk, descent is stopped\t");

src/modules/navigator/rtl.cpp

@@ -237,7 +237,7 @@ void RTL::on_activation()
 		break;
 	}
 
-	_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
+	_navigator->gimbal_neutral_delayed();
 }
 
 void RTL::on_active()