use library in FlightTaskAutoFollowTarget

src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt

@@ -37,5 +37,5 @@ px4_add_library(FlightTaskAutoFollowTarget
 	FlightTaskAutoFollowTarget.cpp
 )
 
-target_link_libraries(FlightTaskAutoFollowTarget PUBLIC FlightTaskAuto follow_target_estimator)
+target_link_libraries(FlightTaskAutoFollowTarget PUBLIC FlightTaskAuto follow_target_estimator gimbal_control)
 target_include_directories(FlightTaskAutoFollowTarget PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp

@@ -59,7 +59,7 @@ FlightTaskAutoFollowTarget::FlightTaskAutoFollowTarget() : _sticks(this)
 
 FlightTaskAutoFollowTarget::~FlightTaskAutoFollowTarget()
 {
-	releaseGimbalControl();
+	_gimbal_control.releaseGimbalControlIfNeeded();
 	_target_estimator.Stop();
 }
 
@@ -428,33 +428,8 @@ bool FlightTaskAutoFollowTarget::update()
 	return true;
 }
 
-void FlightTaskAutoFollowTarget::releaseGimbalControl()
-{
-	// NOTE: If other flight tasks start using gimbal control as well
-	// it might be worth moving this release mechanism to a common base
-	// class for gimbal-control flight tasks
-
-	vehicle_command_s vehicle_command = {};
-	vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
-	vehicle_command.param1 = -3.0f; // Remove control if it had it.
-	vehicle_command.param2 = -3.0f; // Remove control if it had it.
-	vehicle_command.param3 = -1.0f; // Leave unchanged.
-	vehicle_command.param4 = -1.0f; // Leave unchanged.
-
-	vehicle_command.timestamp = hrt_absolute_time();
-	vehicle_command.source_system = _param_mav_sys_id.get();
-	vehicle_command.source_component = _param_mav_comp_id.get();
-	vehicle_command.target_system = _param_mav_sys_id.get();
-	vehicle_command.target_component = _param_mav_comp_id.get();
-	vehicle_command.confirmation = false;
-	vehicle_command.from_external = false;
-
-	_vehicle_command_pub.publish(vehicle_command);
-}
-
 float FlightTaskAutoFollowTarget::pointGimbalAt(const float xy_distance, const float z_distance)
 {
-	gimbal_manager_set_attitude_s msg{};
 	float pitch_down_angle = 0.0f;
 
 	if (PX4_ISFINITE(z_distance)) {
@@ -465,11 +440,10 @@ float FlightTaskAutoFollowTarget::pointGimbalAt(const float xy_distance, const f
 		pitch_down_angle = 0.0;
 	}
 
-	const Quatf q_gimbal = Quatf(Eulerf(0, -pitch_down_angle, 0));
-	q_gimbal.copyTo(msg.q);
-
-	msg.timestamp = hrt_absolute_time();
-	_gimbal_manager_set_attitude_pub.publish(msg);
+	_gimbal_control.acquireGimbalControlIfNeeded();
+	_gimbal_control.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ROLL_PITCH_LOCKED,
+			Quatf(Eulerf(0, -pitch_down_angle, 0)),
+			Vector3f(NAN, NAN, NAN));
 
 	return pitch_down_angle;
 }

src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp

@@ -53,11 +53,10 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/follow_target_status.h>
 #include <uORB/topics/follow_target_estimator.h>
-#include <uORB/topics/gimbal_manager_set_attitude.h>
-#include <uORB/topics/vehicle_command.h>
 
 #include <lib/mathlib/math/filter/second_order_reference_model.hpp>
 #include <motion_planning/VelocitySmoothing.hpp>
+#include <lib/gimbal_control/Gimbal.hpp>
 
 // << Follow Target Behavior related constants >>
 
@@ -252,14 +251,7 @@ protected:
 	 */
 	float pointGimbalAt(const float xy_distance, const float z_distance);
 
-	/**
-	 * Release Gimbal Control
-	 *
-	 * Releases Gimbal Control Authority of Follow-Target Flight Task, to allow other modules / Ground station
-	 * to control the gimbal when the task exits.
-	 * Fore more information on gimbal v2, see https://mavlink.io/en/services/gimbal_v2.html
-	 */
-	void releaseGimbalControl();
+	Gimbal _gimbal_control{this};
 
 	// Sticks object to read in stick commands from the user
 	Sticks _sticks;
@@ -305,6 +297,4 @@ protected:
 	uORB::Subscription _follow_target_estimator_sub{ORB_ID(follow_target_estimator)};
 
 	uORB::Publication<follow_target_status_s> _follow_target_status_pub{ORB_ID(follow_target_status)};
-	uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
-	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 };