From f901dff30d6d00e2054e61b8c651506d828fef18 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 8 Feb 2019 16:08:14 +0100 Subject: [PATCH] Traj Smoothing - make format --- .../FlightTaskManualPositionSmoothVel.cpp | 3 +++ src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index d3ca17e4bd6a..b0ffe8378b15 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -155,11 +155,13 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _smoothing[0].setCurrentPosition(_position(0)); _smoothing[1].setCurrentPosition(_position(1)); } + if (!_position_lock_z_active) { _smoothing[2].setCurrentPosition(_position(2)); } Vector3f pos_sp_smooth; + for (int i = 0; i < 3; ++i) { _smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i)); @@ -175,6 +177,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _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) { diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index 203a64304a51..9f25fdfe15c0 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -92,7 +92,7 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s if (T1_plus >= 0.f && T3_plus >= 0.f) { T1 = T1_plus; - } else if ( T1_minus >= 0.f && T3_minus >= 0.f) { + } else if (T1_minus >= 0.f && T3_minus >= 0.f) { T1 = T1_minus; }