From 728570828f3d974efeceef345c56f7830d0db5a0 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 24 Feb 2023 11:51:17 +0100 Subject: [PATCH] FlightTaskManualAltitude: use StickTiltXY for horizontal stick mapping --- .../tasks/Auto/FlightTaskAuto.cpp | 4 ++-- .../FlightTaskManualAcceleration.cpp | 3 +-- .../FlightTaskManualAltitude.cpp | 23 +++---------------- .../FlightTaskManualAltitude.hpp | 8 +++---- .../FlightTaskManualPosition.cpp | 2 +- 5 files changed, 11 insertions(+), 29 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 5dffbdb6c7..a5d0c00aab 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -240,7 +240,7 @@ void FlightTaskAuto::_prepareLandSetpoints() // User input assisted landing if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { // Stick full up -1 -> stop, stick full down 1 -> double the speed - vertical_speed *= (1 + _sticks.getPositionExpo()(2)); + vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo()); // Only set a yawrate setpoint if weather vane is not active or the yaw stick is out of its dead-zone if (!_weathervane.isActive() || fabsf(_sticks.getYawExpo()) > FLT_EPSILON) { @@ -248,7 +248,7 @@ void FlightTaskAuto::_prepareLandSetpoints() _deltatime); } - _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position, + _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index 2739a71e51..aba0089ece 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -66,8 +66,7 @@ bool FlightTaskManualAcceleration::update() _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, _deltatime); - _stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _yaw_setpoint, - _position, + _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index c4007b1a9a..5e4f673dde 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -118,7 +118,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() float spd_xy = Vector2f(_velocity).length(); // Use presence of horizontal stick inputs as a transition criteria - float stick_xy = Vector2f(_sticks.getPositionExpo().slice<2, 1>(0, 0)).length(); + float stick_xy = Vector2f(_sticks.getPitchRollExpo()).length(); bool stick_input = stick_xy > 0.001f; if (_terrain_hold) { @@ -326,25 +326,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) void FlightTaskManualAltitude::_updateSetpoints() { _updateHeadingSetpoints(); // get yaw setpoint - - // Thrust in xy are extracted directly from stick inputs. A magnitude of - // 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no - // thrust along xy is demanded. The maximum thrust along xy depends on the thrust - // setpoint along z-direction, which is computed in PositionControl.cpp. - - Vector2f sp(_sticks.getPosition().slice<2, 1>(0, 0)); - - _man_input_filter.setParameters(_deltatime, _param_mc_man_tilt_tau.get()); - _man_input_filter.update(sp); - sp = _man_input_filter.getState(); - _rotateIntoHeadingFrame(sp); - - if (sp.longerThan(1.0f)) { - sp.normalize(); - } - - _acceleration_setpoint.xy() = sp * tanf(math::radians(_param_mpc_man_tilt_max.get())) * CONSTANTS_ONE_G; - + _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, + _yaw_setpoint); _updateAltitudeLock(); _respectGroundSlowdown(); } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 4032625be4..062acfc6c1 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -42,6 +42,7 @@ #include "FlightTask.hpp" #include #include "Sticks.hpp" +#include "StickTiltXY.hpp" #include "StickYaw.hpp" #include @@ -75,7 +76,9 @@ protected: void _updateAltitudeLock(); Sticks _sticks{this}; + StickTiltXY _stick_tilt_xy{this}; StickYaw _stick_yaw{this}; + bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, @@ -89,8 +92,7 @@ protected: (ParamFloat) _param_mpc_land_speed, /**< desired downwards speed when approaching the ground */ (ParamFloat) - _param_mpc_tko_speed, /**< desired upwards speed when still close to the ground */ - (ParamFloat) _param_mc_man_tilt_tau + _param_mpc_tko_speed /**< desired upwards speed when still close to the ground */ ) private: bool _isYawInput(); @@ -138,6 +140,4 @@ private: * _dist_to_ground_lock. */ float _dist_to_ground_lock = NAN; - - AlphaFilter _man_input_filter; }; diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp index 0cfe1d4d67..24b98f35ef 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -67,7 +67,7 @@ void FlightTaskManualPosition::_scaleSticks() /* Use same scaling as for FlightTaskManualAltitude */ FlightTaskManualAltitude::_scaleSticks(); - Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0); + Vector2f stick_xy = _sticks.getPitchRollExpo(); Sticks::limitStickUnitLengthXY(stick_xy);