diff --git a/msg/GimbalManagerSetAttitude.msg b/msg/GimbalManagerSetAttitude.msg index d88acca8b6..e1a174fd0b 100644 --- a/msg/GimbalManagerSetAttitude.msg +++ b/msg/GimbalManagerSetAttitude.msg @@ -20,3 +20,5 @@ float32[4] q float32 angular_velocity_x float32 angular_velocity_y float32 angular_velocity_z + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 7b1c24f1dd..dd2ca1da59 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -39,6 +39,7 @@ #include using namespace time_literals; +using namespace matrix; bool FlightTaskManualAccelerationSlow::update() { @@ -128,7 +129,29 @@ bool FlightTaskManualAccelerationSlow::update() FlightTaskManualAltitude::_velocity_constraint_down = velocity_down; FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate); - return FlightTaskManualAcceleration::update(); + bool ret = FlightTaskManualAcceleration::update(); + + // Optimize input-to-video latency gimbal control + if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) { + _gimbal.acquireGimbalControlIfNeeded(); + + // the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward + const float pitchrate_setpoint = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate; + _yawspeed_setpoint = _sticks.getYaw() * yaw_rate; + + _gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, pitchrate_setpoint, _yawspeed_setpoint)); + + if (_gimbal.allAxesLockedConfirmed()) { + // but the vehicle makes sure it stays alligned with the gimbal absolute yaw + _yaw_setpoint = _gimbal.getTelemetryYaw(); + } + + } else { + _gimbal.releaseGimbalControlIfNeeded(); + } + + return ret; } float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value) @@ -136,3 +159,11 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i const int sanitized_index = math::constrain(parameter_value - 1, 0, 5); return _sticks.getAux()(sanitized_index); } + +bool FlightTaskManualAccelerationSlow::haveTakenOff() +{ + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + + return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT; +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index b54d9b765d..66b2de4a8f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -42,6 +42,7 @@ #include "FlightTaskManualAcceleration.hpp" #include "StickAccelerationXY.hpp" +#include "Gimbal.hpp" #include #include @@ -67,9 +68,15 @@ private: bool _velocity_limits_received_before{false}; + // Gimbal control + Gimbal _gimbal{this}; + uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)}; velocity_limits_s _velocity_limits{}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + bool haveTakenOff(); + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, (ParamInt) _param_mc_slow_map_hvel, (ParamInt) _param_mc_slow_map_vvel, @@ -83,6 +90,7 @@ private: (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_man_y_max + (ParamFloat) _param_mpc_man_y_max, + (ParamInt) _param_mc_slow_map_pitch ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c index 4c643d87d8..32558d98ea 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -156,3 +156,17 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f); * @group Multicopter Position Slow Mode */ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f); + +/** + * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode + * + * @value 0 No pitch rate input + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0); diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 6c32116237..44a5e1dba1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp + Gimbal.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp new file mode 100644 index 0000000000..21e97f5fc3 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "Gimbal.hpp" +#include + +using namespace time_literals; +using namespace matrix; + +Gimbal::Gimbal(ModuleParams *parent) : + ModuleParams(parent) +{} + +Gimbal::~Gimbal() +{ + releaseGimbalControlIfNeeded(); +} + +bool Gimbal::checkForTelemetry(const hrt_abstime now) +{ + if (_gimbal_device_attitude_status_sub.updated()) { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status)) { + _telemtry_timestamp = gimbal_device_attitude_status.timestamp; + _telemetry_flags = gimbal_device_attitude_status.device_flags; + _telemetry_yaw = Eulerf(Quatf(gimbal_device_attitude_status.q)).psi(); + } + } + + return now < _telemtry_timestamp + 2_s; +} + +void Gimbal::acquireGimbalControlIfNeeded() +{ + if (!_have_gimbal_control) { + _have_gimbal_control = true; + + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _param_mav_sys_id.get(); + vehicle_command.param2 = _param_mav_comp_id.get(); + 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_sys_id.get(); + vehicle_command.from_external = false; + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::releaseGimbalControlIfNeeded() +{ + if (_have_gimbal_control) { + _have_gimbal_control = false; + + // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior + publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, + Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, 0.f, 0.f)); + + // Release gimbal + 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.from_external = false; + 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_pub.publish(vehicle_command); + } +} + +void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates) +{ + gimbal_manager_set_attitude_s gimbal_setpoint{}; + gimbal_setpoint.origin_sysid = _param_mav_sys_id.get(); + gimbal_setpoint.origin_compid = _param_mav_comp_id.get(); + gimbal_setpoint.flags = gimbal_flags; + q_gimbal_setpoint.copyTo(gimbal_setpoint.q); + gimbal_setpoint.angular_velocity_x = gimbal_rates(0); + gimbal_setpoint.angular_velocity_y = gimbal_rates(1); + gimbal_setpoint.angular_velocity_z = gimbal_rates(2); + gimbal_setpoint.timestamp = hrt_absolute_time(); + _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp new file mode 100644 index 0000000000..938ef32aa2 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file Gimbal.hpp + * @brief Gimbal interface with flight tasks + * @author Pernilla Wikström + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Sticks.hpp" + + +class Gimbal : public ModuleParams +{ +public: + Gimbal(ModuleParams *parent); + ~Gimbal(); + + bool checkForTelemetry(const hrt_abstime now); + void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates); + void acquireGimbalControlIfNeeded(); + void releaseGimbalControlIfNeeded(); + float getTelemetryYaw() { return _telemetry_yaw; } + bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } + + static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK; + static constexpr uint16_t FLAGS_ROLL_PITCH_LOCKED = + gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + +private: + bool _have_gimbal_control{false}; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + hrt_abstime _telemtry_timestamp{}; + uint16_t _telemetry_flags{}; + float _telemetry_yaw{}; + + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id + ) +}; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index e20bf6c552..aef348ca53 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -43,6 +43,8 @@ #include #include +using namespace matrix; + namespace gimbal { @@ -936,21 +938,19 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con if (set_manual_control.origin_sysid == control_data.sysid_primary_control && set_manual_control.origin_compid == control_data.compid_primary_control) { - const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) - ? - matrix::Quatf( - matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) - : - matrix::Quatf(NAN, NAN, NAN, NAN); + Quatf q(NAN, NAN, NAN, NAN); - const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && - PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); + if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) { + q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)); + } + + Vector3f angular_velocity(NAN, NAN, NAN); + + if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) { + angular_velocity = Vector3f(0.0f, + set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch), + set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw)); + } _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, angular_velocity, set_manual_control.timestamp);