From a7401b47ce6df9628a0c9ebfbc4470e5dd9accba Mon Sep 17 00:00:00 2001 From: Pernilla Date: Tue, 4 Feb 2025 17:32:32 +0100 Subject: [PATCH] use library in FlightTaskAutoFollowTarget --- .../tasks/AutoFollowTarget/CMakeLists.txt | 2 +- .../FlightTaskAutoFollowTarget.cpp | 36 +++---------------- .../FlightTaskAutoFollowTarget.hpp | 14 ++------ 3 files changed, 8 insertions(+), 44 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt index 9abd0c8db2..54fffa7558 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt +++ b/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}) diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp index 4699f1349f..fc77f10d88 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp +++ b/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; } diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp index 1eb4ba8557..90c331bfe9 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp @@ -53,11 +53,10 @@ #include #include #include -#include -#include #include #include +#include // << 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_pub{ORB_ID(follow_target_status)}; - uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; - uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; };