From fe4e3ffe00e0709d36c1bee266d76ebb1e2cc1fb Mon Sep 17 00:00:00 2001 From: Pernilla Date: Mon, 3 Feb 2025 17:44:41 +0100 Subject: [PATCH] Move gimbal flight mode manager utility to library --- src/lib/CMakeLists.txt | 1 + src/lib/gimbal_control/CMakeLists.txt | 34 +++++++++++++++++++ .../Utility => lib/gimbal_control}/Gimbal.cpp | 12 +++++++ .../Utility => lib/gimbal_control}/Gimbal.hpp | 19 ++++++++--- .../tasks/Utility/CMakeLists.txt | 1 - 5 files changed, 62 insertions(+), 5 deletions(-) create mode 100644 src/lib/gimbal_control/CMakeLists.txt rename src/{modules/flight_mode_manager/tasks/Utility => lib/gimbal_control}/Gimbal.cpp (92%) rename src/{modules/flight_mode_manager/tasks/Utility => lib/gimbal_control}/Gimbal.hpp (89%) diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8f58109b89..5cf37bced2 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -52,6 +52,7 @@ add_subdirectory(dataman_client EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) +add_subdirectory(gimbal_control EXCLUDE_FROM_ALL) add_subdirectory(heatshrink EXCLUDE_FROM_ALL) add_subdirectory(hysteresis EXCLUDE_FROM_ALL) add_subdirectory(l1 EXCLUDE_FROM_ALL) diff --git a/src/lib/gimbal_control/CMakeLists.txt b/src/lib/gimbal_control/CMakeLists.txt new file mode 100644 index 0000000000..781281e2df --- /dev/null +++ b/src/lib/gimbal_control/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(gimbal_control Gimbal.cpp) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/lib/gimbal_control/Gimbal.cpp similarity index 92% rename from src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp rename to src/lib/gimbal_control/Gimbal.cpp index 21e97f5fc3..0930632bb3 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp +++ b/src/lib/gimbal_control/Gimbal.cpp @@ -124,3 +124,15 @@ void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, gimbal_setpoint.timestamp = hrt_absolute_time(); _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); } + +void Gimbal::setNeutral() +{ + vehicle_command_s vehicle_command = {}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW; + vehicle_command.param1 = NAN; + vehicle_command.param2 = NAN; + vehicle_command.param3 = NAN; + vehicle_command.param4 = NAN; + vehicle_command.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL; + _vehicle_command_pub.publish(vehicle_command); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/lib/gimbal_control/Gimbal.hpp similarity index 89% rename from src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp rename to src/lib/gimbal_control/Gimbal.hpp index 938ef32aa2..f361662fd5 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp +++ b/src/lib/gimbal_control/Gimbal.hpp @@ -33,7 +33,7 @@ /** * @file Gimbal.hpp - * @brief Gimbal interface with flight tasks + * @brief Gimbal library to interface with other units * @author Pernilla Wikström */ @@ -47,9 +47,9 @@ #include #include -#include "Sticks.hpp" - - +/** +* Fore more information on gimbal v2, see https://mavlink.io/en/services/gimbal_v2.html + **/ class Gimbal : public ModuleParams { public: @@ -60,8 +60,19 @@ public: void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, const matrix::Quatf &q_gimbal_setpoint, const matrix::Vector3f &gimbal_rates); + /** + * Acquire Gimbal Control Authority to restrict other modules / Ground station + * to control the gimbal until the gimbal is released. + */ void acquireGimbalControlIfNeeded(); + + /** + * Releases Gimbal Control Authority, to allow other modules / Ground station + * to control the gimbal. + */ void releaseGimbalControlIfNeeded(); + + void setNeutral(); float getTelemetryYaw() { return _telemetry_yaw; } bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index dd17b3abcc..911da11d26 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp - Gimbal.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib)