From 70ddb8049a8800071bc4dc3630b06808fbd08801 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
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 d3ca17e4bd..b0ffe8378b 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 203a64304a..9f25fdfe15 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;
}
--
GitLab