dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ManualSmoothVel - Split position lock condition and flag action

This commit is contained in:
bresch 2019-02-19 14:07:27 +01:00
parent db0283ef21
commit f229444722
1 changed files with 13 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
View File

@ -180,18 +180,28 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
if (Vector2f(_vel_sp_smooth).length() < 0.01f &&
Vector2f(_acceleration_setpoint).length() < .2f &&
sticks_expo_xy.length() <= FLT_EPSILON) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
_position_lock_xy_active = true;
}
if (fabsf(_vel_sp_smooth(2)) < 0.01f &&
fabsf(_acceleration_setpoint(2)) < .2f &&
fabsf(_sticks_expo(2)) <= FLT_EPSILON) {
_position_setpoint_z_locked = pos_sp_smooth(2);
_position_lock_z_active = true;
}
// Set valid position setpoint while in position lock.
// When the position lock condition above is false, it does not
// mean that the unlock condition is true. This is why
// we are checking the lock flag here.
if (_position_lock_xy_active) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
}
if (_position_lock_z_active) {
_position_setpoint_z_locked = pos_sp_smooth(2);
}
_position_setpoint(0) = _position_setpoint_xy_locked(0);
_position_setpoint(1) = _position_setpoint_xy_locked(1);
_position_setpoint(2) = _position_setpoint_z_locked;