From d168d5049f55c393bf7f33107fa17a8064b70e02 Mon Sep 17 00:00:00 2001 From: baumanta Date: Tue, 20 Aug 2019 14:17:02 +0200 Subject: [PATCH] check vel_setpoint instead of stick input for position lock --- .../FlightTaskManualPositionSmoothVel.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index 62ac3c1857..a0bb693460 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -169,9 +169,9 @@ 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 sticks_expo_xy = Vector2f(&_sticks_expo(0)); + Vector2f velocity_setpoint_xy = Vector2f(&_velocity_setpoint(0)); - if (sticks_expo_xy.length() > FLT_EPSILON) { + if (velocity_setpoint_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 +183,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _position_lock_xy_active = false; } - if (fabsf(_sticks_expo(2)) > FLT_EPSILON) { + if (fabsf(_velocity_setpoint(2)) > FLT_EPSILON) { if (_position_lock_z_active) { _smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback( 2)); // Start the trajectory at the current velocity setpoint @@ -233,13 +233,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() // Check for position lock transition if (Vector2f(_vel_sp_smooth).length() < 0.1f && Vector2f(_acceleration_setpoint).length() < .2f && - sticks_expo_xy.length() <= FLT_EPSILON) { + velocity_setpoint_xy.length() <= FLT_EPSILON) { _position_lock_xy_active = true; } if (fabsf(_vel_sp_smooth(2)) < 0.1f && fabsf(_acceleration_setpoint(2)) < .2f && - fabsf(_sticks_expo(2)) <= FLT_EPSILON) { + fabsf(_velocity_setpoint(2)) <= FLT_EPSILON) { _position_lock_z_active = true; }