use library in navigator

src/modules/navigator/CMakeLists.txt

@@ -70,4 +70,5 @@ px4_add_module(
 		motion_planning
 		mission_feasibility_checker
 		rtl_time_estimator
+		gimbal_control
 	)

src/modules/navigator/land.cpp

@@ -73,9 +73,9 @@ Land::on_activation()
 	_navigator->reset_cruising_speed();
 
 	// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
-	_navigator->acquire_gimbal_control();
-	_navigator->set_gimbal_neutral();
-	_navigator->release_gimbal_control();
+	_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+	_navigator->_gimbal_control.setNeutral();
+	_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 
 }
 

src/modules/navigator/mission_base.cpp

@@ -180,8 +180,8 @@ MissionBase::on_inactivation()
 	_navigator->disable_camera_trigger();
 
 	_navigator->stop_capturing_images();
-	_navigator->set_gimbal_neutral(); // point forward
-	_navigator->release_gimbal_control();
+	_navigator->_gimbal_control.setNeutral(); // point forward
+	_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 
 	if (_navigator->get_precland()->is_activated()) {
 		_navigator->get_precland()->on_inactivation();
@@ -683,9 +683,9 @@ void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s
 			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
 
 			// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
-			_navigator->acquire_gimbal_control();
-			_navigator->set_gimbal_neutral();
-			_navigator->release_gimbal_control();
+			_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+			_navigator->_gimbal_control.setNeutral();
+			_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 
 		} else {
 

src/modules/navigator/mission_block.cpp

@@ -524,7 +524,7 @@ MissionBlock::issue_command(const mission_item_s &item)
 
 	// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
 	if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) {
-		_navigator->acquire_gimbal_control();
+		_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
 	}
 
 	// Mission item's NAV_CMD enums directly map to the according vehicle command

src/modules/navigator/navigator.h

@@ -60,6 +60,7 @@
 #if CONFIG_NAVIGATOR_ADSB
 #include <lib/adsb/AdsbConflict.h>
 #endif // CONFIG_NAVIGATOR_ADSB
+#include <lib/gimbal_control/Gimbal.hpp>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/events.h>
 #include <px4_platform_common/module.h>
@@ -277,9 +278,7 @@ public:
 
 	bool force_vtol();
 
-	void acquire_gimbal_control();
-	void release_gimbal_control();
-	void set_gimbal_neutral();
+	Gimbal _gimbal_control{this};
 
 	void preproject_stop_point(double &lat, double &lon);
 

src/modules/navigator/navigator_main.cpp

@@ -1543,29 +1543,6 @@ void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_
 	_vehicle_cmd_ack_pub.publish(command_ack);
 }
 
-void Navigator::acquire_gimbal_control()
-{
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vcmd.param1 = _vstatus.system_id;
-	vcmd.param2 = _vstatus.component_id;
-	vcmd.param3 = -1.0f; // Leave unchanged.
-	vcmd.param4 = -1.0f; // Leave unchanged.
-	publish_vehicle_cmd(&vcmd);
-}
-
-void Navigator::release_gimbal_control()
-{
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vcmd.param1 = -3.0f; // Remove control if it had it.
-	vcmd.param2 = -3.0f; // Remove control if it had it.
-	vcmd.param3 = -1.0f; // Leave unchanged.
-	vcmd.param4 = -1.0f; // Leave unchanged.
-	publish_vehicle_cmd(&vcmd);
-}
-
-
 void
 Navigator::stop_capturing_images()
 {
@@ -1629,18 +1606,6 @@ void Navigator::disable_camera_trigger()
 	publish_vehicle_cmd(&cmd);
 }
 
-void Navigator::set_gimbal_neutral()
-{
-	vehicle_command_s vcmd = {};
-	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
-	vcmd.param1 = NAN;
-	vcmd.param2 = NAN;
-	vcmd.param3 = NAN;
-	vcmd.param4 = NAN;
-	vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
-	publish_vehicle_cmd(&vcmd);
-}
-
 void Navigator::sendWarningDescentStoppedDueToTerrain()
 {
 	mavlink_log_critical(&_mavlink_log_pub, "Terrain collision risk, descent is stopped\t");

src/modules/navigator/rtl.cpp

@@ -238,9 +238,9 @@ void RTL::on_activation()
 	}
 
 	// set gimbal to neutral position (level with horizon) to reduce change of damage on landing
-	_navigator->acquire_gimbal_control();
-	_navigator->set_gimbal_neutral();
-	_navigator->release_gimbal_control();
+	_navigator->_gimbal_control.acquireGimbalControlIfNeeded();
+	_navigator->_gimbal_control.setNeutral();
+	_navigator->_gimbal_control.releaseGimbalControlIfNeeded();
 }
 
 void RTL::on_active()