From 57e5e3cb6f2b499ef8b32d84727c73c0b4d29c69 Mon Sep 17 00:00:00 2001 From: Balduin Date: Tue, 22 Jul 2025 17:36:47 +0200 Subject: [PATCH] initial fix: param -1 to turn off, also turn off if position invalid, where turning off = nudging from descend problems: - if disabled only due to param -1, this gives acceleration commands but they do not translate to the drone moving. what is missing? - if disabled due to invalid local pos, the setpoints are rejected by mpc as invalid. --- .../tasks/Auto/FlightTaskAuto.cpp | 57 ++++++++++++++----- .../tasks/Auto/FlightTaskAuto.hpp | 2 + .../multicopter_nudging_params.c | 6 +- 3 files changed, 48 insertions(+), 17 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 1ae5d733f0..beebd21578 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -266,27 +266,54 @@ void FlightTaskAuto::_prepareLandSetpoints() Vector2f sticks_ne = sticks_xy; Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading); - const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(), - _param_mpc_land_radius.get(), sticks_ne); - float max_speed; + const bool position_valid = !isnan(_position(0)) && !isnan(_position(1)); - if (PX4_ISFINITE(distance_to_circle)) { - max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(), - _stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f); + if (_param_mpc_land_radius.get() > 0 && position_valid) { - if (max_speed < 0.5f) { + PX4_INFO(" fancy nudging"); + + // Only allow nudging if it points towards of the circle defined by MPC_LAND_RADIUS + // This is only allowed for valid local position. + + const float distance_to_circle = math::trajectory::getMaxDistanceToCircle(_position.xy(), _initial_land_position.xy(), + _param_mpc_land_radius.get(), sticks_ne); + float max_speed; + + + if (PX4_ISFINITE(distance_to_circle)) { + PX4_INFO(" circle distance finite"); + max_speed = math::trajectory::computeMaxSpeedFromDistance(_stick_acceleration_xy.getMaxJerk(), + _stick_acceleration_xy.getMaxAcceleration(), distance_to_circle, 0.f); + + if (max_speed < 0.5f) { + sticks_xy.setZero(); + } + + } else { + max_speed = 0.f; sticks_xy.setZero(); } - } else { - max_speed = 0.f; - sticks_xy.setZero(); - } + PX4_INFO(" max speed: %.2f", (double) max_speed); - _stick_acceleration_xy.setVelocityConstraint(max_speed); - _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, - _velocity_setpoint_feedback.xy(), _deltatime); - _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); + _stick_acceleration_xy.setVelocityConstraint(max_speed); + _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, + _velocity_setpoint_feedback.xy(), _deltatime); + _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); + } else { + // Nudging unrestricted, also works for invalid local pos + PX4_INFO(" basic nudging"); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime); + _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, + _yaw_setpoint); + PX4_INFO(" accel sp: (%.2f, %.2f)", (double) _acceleration_setpoint(0), (double) _acceleration_setpoint(1)); + + + // Stick full up -1 -> stop, stick full down 1 -> double the value + _velocity_setpoint(2) *= (1 - _sticks.getThrottleZeroCenteredExpo()); + _acceleration_setpoint(2) -= _sticks.getThrottleZeroCentered() * 10.f; + + } } else { // Make sure we have a valid land position even in the case we loose RC while amending it diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 00610d910c..c80187eb23 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -53,6 +53,7 @@ #include #include "Sticks.hpp" #include "StickAccelerationXY.hpp" +#include "StickTiltXY.hpp" /** * This enum has to agree with position_setpoint_s type definition @@ -145,6 +146,7 @@ protected: Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; StickAccelerationXY _stick_acceleration_xy{this}; + StickTiltXY _stick_tilt_xy{this}; StickYaw _stick_yaw{this}; matrix::Vector3f _land_position; float _land_heading; diff --git a/src/modules/mc_pos_control/multicopter_nudging_params.c b/src/modules/mc_pos_control/multicopter_nudging_params.c index c431e88fd0..ed1909026f 100644 --- a/src/modules/mc_pos_control/multicopter_nudging_params.c +++ b/src/modules/mc_pos_control/multicopter_nudging_params.c @@ -56,10 +56,12 @@ PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); * When nudging is enabled (see MPC_LAND_RC_HELP), this controls * the maximum allowed horizontal displacement from the original landing point. * + * Setting the radius to -1 will disable this feature. Only works while local position is valid. + * * @unit m - * @min 0 + * @min -1 * @decimal 0 * @increment 1 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); +PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, -1.0f);