From b7e2a9c320a001ab0f9adf179699c65916b2989e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 30 Jun 2023 11:46:33 +0200 Subject: [PATCH] FlightTaskAltitude: remove duplicate, deprecated yaw stick handling --- .../FlightTaskManualAltitude.cpp | 41 +------------------ .../FlightTaskManualAltitude.hpp | 5 --- 2 files changed, 2 insertions(+), 44 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 90d5dccb1f..0a2704d68a 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -90,10 +90,6 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() void FlightTaskManualAltitude::_scaleSticks() { - // Use stick input with deadzone, exponential curve and first order lpf for yawspeed - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); - // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() : _param_mpc_z_vel_max_up.get(); @@ -273,40 +269,6 @@ void FlightTaskManualAltitude::_respectGroundSlowdown() } } -void FlightTaskManualAltitude::_updateHeadingSetpoints() -{ - if (_isYawInput() || !_is_yaw_good_for_control) { - _unlockYaw(); - - } else { - _lockYaw(); - } -} - -bool FlightTaskManualAltitude::_isYawInput() -{ - /* - * A threshold larger than FLT_EPSILON is required because the - * _yawspeed_setpoint comes from an IIR filter and takes too much - * time to reach zero. - */ - return fabsf(_yawspeed_setpoint) > 0.001f; -} - -void FlightTaskManualAltitude::_unlockYaw() -{ - // no fixed heading when rotating around yaw by stick - _yaw_setpoint = NAN; -} - -void FlightTaskManualAltitude::_lockYaw() -{ - // hold the current heading when no more rotation commanded - if (!PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - } -} - void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) { // Only reset the yaw setpoint when the heading is locked @@ -317,7 +279,8 @@ void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) void FlightTaskManualAltitude::_updateSetpoints() { - _updateHeadingSetpoints(); // get yaw setpoint + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, + _is_yaw_good_for_control, _deltatime); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); _updateAltitudeLock(); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 0b9fbe72a3..fdac174e37 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -56,7 +56,6 @@ public: bool update() override; protected: - void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ @@ -89,10 +88,6 @@ protected: _param_mpc_tko_speed /**< desired upwards speed when still close to the ground */ ) private: - bool _isYawInput(); - void _unlockYaw(); - void _lockYaw(); - /** * Terrain following. * During terrain following, the position setpoint is adjusted