|
|
|
@ -169,9 +169,9 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
@@ -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()
@@ -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()
@@ -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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|