From 67c08460689726040a85b44424ad8da43f6d7f3f Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 25 Oct 2018 15:55:12 +0200
Subject: [PATCH] Vel smooth - Improve position lock/unlock detection
---
.../FlightTaskManualPositionSmoothVel.cpp | 19 ++++++++-----------
1 file changed, 8 insertions(+), 11 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 199c57431e..9399e78cc2 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -75,7 +75,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
}
Vector2f vel_xy_sp = Vector2f(&_velocity_setpoint(0));
- Vector2f vel_xy_sp_smooth = Vector2f(&_vel_sp_smooth(0));
float jerk[3] = {_jerk_max.get(), _jerk_max.get(), _jerk_max.get()};
float jerk_xy = _jerk_max.get();
@@ -126,22 +125,20 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
Vector3f accel_sp_smooth;
for (int i = 0; i < 3; ++i) {
- _smoothing[i].setCurrentPosition(_position(i));
+ if (!_position_lock_xy_active) {
+ _smoothing[i].setCurrentPosition(_position(i));
+ }
+
_smoothing[i].integrate(accel_sp_smooth(i), _vel_sp_smooth(i), pos_sp_smooth(i));
_velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward
}
// 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 &&
+ if (Vector2f(_vel_sp_smooth).length() < 0.01f &&
+ Vector2f(accel_sp_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_setpoint_xy_locked(0) = pos_sp_smooth(0);
+ _position_setpoint_xy_locked(1) = pos_sp_smooth(1);
_position_lock_xy_active = true;
}
--
GitLab