fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 06:47:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

check vel_setpoint instead of stick input for position lock

Browse Source
This commit is contained in:
baumanta
2019-08-20 14:17:02 +02:00
committed by Julian Kent
parent 5a736c9af8
commit d168d5049f
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+5 -5
View File
@@ -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;
}