From f4b40865afd8fdb10f1693a870e411909c903cdc Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 22 Aug 2019 15:54:35 +0200 Subject: [PATCH] SmoothVel - Fix altitude lock logic. It was broken because _velocity_setpoint is used for input and output and was assumed to be the input at a place where it was already overwitten To clarify this, the input setpoint is renamed "target" --- .../FlightTaskManualAltitudeSmoothVel.cpp | 6 ++++-- .../FlightTaskManualPositionSmoothVel.cpp | 11 ++++++----- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index 0243fecdd7..b2e5ec473b 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -130,7 +130,9 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() * is used to set current velocity of the trajectory. */ - if (fabsf(_velocity_setpoint(2)) > FLT_EPSILON) { + const float velocity_target_z = _velocity_setpoint(2); + + if (fabsf(velocity_target_z) > FLT_EPSILON) { if (_position_lock_z_active) { _smoothing.setCurrentVelocity(_velocity_setpoint_feedback( 2)); // Start the trajectory at the current velocity setpoint @@ -159,7 +161,7 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() if (fabsf(_vel_sp_smooth) < 0.1f && fabsf(_acceleration_setpoint(2)) < .2f && - fabsf(_velocity_setpoint(2)) <= FLT_EPSILON) { + fabsf(velocity_target_z) <= FLT_EPSILON) { _position_lock_z_active = true; } diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index a0bb693460..d73b012ee6 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -169,9 +169,10 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller * is used to set current velocity of the trajectory. */ - Vector2f velocity_setpoint_xy = Vector2f(&_velocity_setpoint(0)); + const Vector2f velocity_target_xy = Vector2f(_velocity_setpoint); + const float velocity_target_z = _velocity_setpoint(2); - if (velocity_setpoint_xy.length() > FLT_EPSILON) { + if (velocity_target_xy.length() > FLT_EPSILON) { if (_position_lock_xy_active) { _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback( 0)); // Start the trajectory at the current velocity setpoint @@ -183,7 +184,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _position_lock_xy_active = false; } - if (fabsf(_velocity_setpoint(2)) > FLT_EPSILON) { + if (fabsf(velocity_target_z) > FLT_EPSILON) { if (_position_lock_z_active) { _smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback( 2)); // Start the trajectory at the current velocity setpoint @@ -233,13 +234,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() // Check for position lock transition if (Vector2f(_vel_sp_smooth).length() < 0.1f && Vector2f(_acceleration_setpoint).length() < .2f && - velocity_setpoint_xy.length() <= FLT_EPSILON) { + velocity_target_xy.length() <= FLT_EPSILON) { _position_lock_xy_active = true; } if (fabsf(_vel_sp_smooth(2)) < 0.1f && fabsf(_acceleration_setpoint(2)) < .2f && - fabsf(_velocity_setpoint(2)) <= FLT_EPSILON) { + fabsf(velocity_target_z) <= FLT_EPSILON) { _position_lock_z_active = true; }