Navigator: delay neutral gimbal command (#25551)

* Set gimbal neutral after delay

src/modules/navigator/land.cpp

@@ -72,11 +72,7 @@ Land::on_activation()
 	// reset cruising speed to default
 	_navigator->reset_cruising_speed();
 
-	// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
+	_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
-	_navigator->acquire_gimbal_control();
-	_navigator->set_gimbal_neutral();
-	_navigator->release_gimbal_control();
-
 }
 
 void

src/modules/navigator/mission_base.cpp

@@ -180,8 +180,10 @@ MissionBase::on_inactivation()
 	_navigator->disable_camera_trigger();
 
 	_navigator->stop_capturing_images();
-	_navigator->set_gimbal_neutral(); // point forward
+
-	_navigator->release_gimbal_control();
+	if (!_navigator->get_land_detected()->landed) {
+		_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
+	}
 
 	if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
@@ -698,10 +700,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);
 
-			// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
+			_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
-			_navigator->acquire_gimbal_control();
-			_navigator->set_gimbal_neutral();
-			_navigator->release_gimbal_control();
 
 		} else {
 

src/modules/navigator/navigator.h

@@ -282,6 +282,13 @@ public:
 	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 preproject_stop_point(double &lat, double &lon);
 
 	void stop_capturing_images();
@@ -391,6 +398,9 @@ 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};
+
 	// update subscriptions
 	void params_update();
 

src/modules/navigator/navigator_main.cpp

@@ -904,6 +904,8 @@ void Navigator::run()
 			publish_mission_result();
 		}
 
+		neutralize_gimbal_if_control_activated();
+
 		publish_navigator_status();
 
 		publish_distance_sensor_mode_request();
@@ -1629,6 +1631,27 @@ 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,10 +237,7 @@ void RTL::on_activation()
 		break;
 	}
 
-	// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
+	_navigator->activate_set_gimbal_neutral_timer(hrt_absolute_time());
-	_navigator->acquire_gimbal_control();
-	_navigator->set_gimbal_neutral();
-	_navigator->release_gimbal_control();
 }
 
 void RTL::on_active()