fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 11:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

SmoothVel - Fix altitude lock logic.

Browse Source 
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"
This commit is contained in:
bresch
2019-08-22 16:43:41 +02:00
committed by Julian Oes
parent dd2d5c029f
commit f4b40865af
2 changed files with 10 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp
+4 -2
View File
@@ -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;
}