From 0d51c837353f3df0df8a925226658e6c455f4efe Mon Sep 17 00:00:00 2001 From: Pernilla Date: Thu, 23 Jan 2025 11:52:14 +0100 Subject: [PATCH] Gimbal: getting pitch input from gimbal_manager_set_manual_control FlightTaskManualAccelerationSlow: avoid misalignment of gimbal and vehicle before takeoff --- .../FlightTaskManualAccelerationSlow.cpp | 20 ++++++++++++++----- .../FlightTaskManualAccelerationSlow.hpp | 16 ++++++++------- .../flight_task_acceleration_slow_params.c | 14 ------------- .../tasks/Utility/Gimbal.cpp | 20 +++++++++++++++++++ .../tasks/Utility/Gimbal.hpp | 18 +++++++++++++++-- 5 files changed, 60 insertions(+), 28 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index b1abc6ccf5b9..070e9898217e 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -132,16 +132,18 @@ bool FlightTaskManualAccelerationSlow::update() bool ret = FlightTaskManualAcceleration::update(); // Optimize input-to-video latency gimbal control - if (_gimbal.checkForTelemetry(_time_stamp_current)) { + if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) { _gimbal.acquireGimbalControlIfNeeded(); - const float pitchrate_gimbal = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate; - _yawspeed_setpoint = shapeYawStickToGimbalRate(_sticks.getYaw(), yaw_rate); + // the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward + const float pitchrate_gimbal = shapeUnitlessStickToGimbalRate(_gimbal.getPitch(_time_stamp_current), yaw_rate); + _yawspeed_setpoint = shapeUnitlessStickToGimbalRate(_sticks.getYaw(), yaw_rate); _gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN), Vector3f(NAN, pitchrate_gimbal, _yawspeed_setpoint)); if (_gimbal.allAxesLockedConfirmed()) { + // but the vehicle makes sure it stays alligned with the gimbal absolute yaw _yaw_setpoint = _gimbal.getTelemetryYaw(); } @@ -158,7 +160,15 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i return _sticks.getAux()(sanitized_index); } -float FlightTaskManualAccelerationSlow::shapeYawStickToGimbalRate(float stick_yaw, float maximum_yawrate) +float FlightTaskManualAccelerationSlow::shapeUnitlessStickToGimbalRate(float stick_input, float maximum_rate) { - return stick_yaw * maximum_yawrate; + return stick_input * maximum_rate; +} + +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 d408d16004d7..cef1553cb069 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -67,12 +67,12 @@ class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration float getInputFromSanitizedAuxParameterIndex(int parameter_value); /** - * Input shaping of yaw stick input for gimbal setpoint - * @param stick_yaw raw calibrated yaw stick value [-1, 1] - * @param maximum_yawrate yaw rate [rad/s] with maximum stick deflection - * @return gimbal yaw rate setpoint [rad/s] positive clockwise + * Input shaping of unitless stick input for gimbal setpoint + * @param stick_input raw calibrated stick value [-1, 1] + * @param maximum_rate rate [rad/s] with maximum stick deflection + * @return gimbal rate setpoint [rad/s] positive clockwise */ - float shapeYawStickToGimbalRate(float stick_yaw, float maximum_yawrate); + float shapeUnitlessStickToGimbalRate(float stick_input, float maximum_rate); bool _velocity_limits_received_before{false}; @@ -82,6 +82,9 @@ class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration 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, @@ -95,7 +98,6 @@ class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration (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, - (ParamInt) _param_mc_slow_map_pitch + (ParamFloat) _param_mpc_man_y_max ) }; 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 32558d98ead9..4c643d87d876 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,17 +156,3 @@ 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/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp index 21e97f5fc3fa..e6c0ab6afad8 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -124,3 +124,23 @@ void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, gimbal_setpoint.timestamp = hrt_absolute_time(); _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); } + +float Gimbal::getPitch(const hrt_abstime now) +{ + if (_gimbal_manager_set_manual_control_sub.updated()) { + gimbal_manager_set_manual_control_s gimbal_manager_set_manual_control{}; + + if (_gimbal_manager_set_manual_control_sub.copy(&gimbal_manager_set_manual_control) + && gimbal_manager_set_manual_control.origin_compid != _param_mav_comp_id.get() + && gimbal_manager_set_manual_control.origin_sysid != _param_mav_sys_id.get()) { + _set_manual_control_timestamp = gimbal_manager_set_manual_control.timestamp; + _set_manual_control_pitchrate = gimbal_manager_set_manual_control.pitch_rate; + } + } + + if (now > _set_manual_control_timestamp + 2_s) { + _set_manual_control_pitchrate = 0.f; + } + + return _set_manual_control_pitchrate; +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp index e136f56bb86f..2b6d29310217 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -45,10 +45,12 @@ #include #include #include +#include #include #include "Sticks.hpp" + class Gimbal : public ModuleParams { public: @@ -64,6 +66,15 @@ class Gimbal : public ModuleParams float getTelemetryYaw() { return _telemetry_yaw; } bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } + /** + * Workaround to mix commands from the gcs which should be + * ignored in gimbal manager + * @param now: current time stamp in loop [ms] + * @return raw calibrated stick value [-1, 1] if + * last sample was within expected time, otherwise NAN. + */ + float getPitch(const hrt_abstime now); + 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; @@ -75,11 +86,14 @@ class Gimbal : public ModuleParams bool _have_gimbal_control{false}; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; - uint16_t _telemetry_flags{}; hrt_abstime _telemtry_timestamp{}; + uint16_t _telemetry_flags{}; float _telemetry_yaw{}; - // Publications + uORB::Subscription _gimbal_manager_set_manual_control_sub{ORB_ID(gimbal_manager_set_manual_control)}; + hrt_abstime _set_manual_control_timestamp{}; + float _set_manual_control_pitchrate{}; + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)};