From d9edcfdc06f623aeac03998c9293d5a0291f02c9 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 17 Oct 2018 16:33:03 +0200
Subject: [PATCH] Trajectory - format style
---
src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp | 3 ++-
.../FlightTaskManualPositionSmoothVel.cpp | 11 ++++++++---
.../FlightTasks/tasks/Utility/VelocitySmoothing.cpp | 2 +-
3 files changed, 11 insertions(+), 5 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index ca98239ef5..9501a45359 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -147,7 +147,8 @@ public:
*/
virtual void setYawHandler(WeatherVane *ext_yaw_handler) {};
- void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
+ void updateVelocityControllerIO(const matrix::Vector3f &vel_sp,
+ const matrix::Vector3f &thrust_sp) {_velocity_setpoint_feedback = vel_sp; _thrust_setpoint_feedback = thrust_sp; }
protected:
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 5a2b215e88..199c57431e 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -102,13 +102,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
* is used to set current velocity of the trajectory.
*/
Vector2f sticks_expo_xy = Vector2f(&_sticks_expo(0));
+
if (sticks_expo_xy.length() > FLT_EPSILON) {
if (_position_lock_xy_active) {
- _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); // Start the trajectory at the current velocity setpoint
+ _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(
+ 0)); // Start the trajectory at the current velocity setpoint
_smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
}
+
_position_lock_xy_active = false;
}
@@ -130,13 +133,15 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
// Check for position lock transition
Vector2f accel_setpoint_xy_smooth = Vector2f(&accel_sp_smooth(0));
+
if (vel_xy_sp_smooth.length() < 0.002f &&
- accel_setpoint_xy_smooth.length() < .2f &&
- sticks_expo_xy.length() <= FLT_EPSILON) {
+ accel_setpoint_xy_smooth.length() < .2f &&
+ sticks_expo_xy.length() <= FLT_EPSILON) {
if (!_position_lock_xy_active) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_position_setpoint_xy_locked(1) = pos_sp_smooth(1);
}
+
_position_lock_xy_active = true;
}
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index d7a09dedb6..b3e2e329a3 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -220,7 +220,7 @@ void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpo
integrate(_dt, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
}
-void VelocitySmoothing::integrate(float dt,float &accel_setpoint_smooth, float &vel_setpoint_smooth,
+void VelocitySmoothing::integrate(float dt, float &accel_setpoint_smooth, float &vel_setpoint_smooth,
float &pos_setpoint_smooth)
{
/* Integrate the trajectory */
--
GitLab