From 57eef6b170c673156c48ab416a3d7d9171c36e3f Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 31 Jan 2019 11:46:43 +0100
Subject: [PATCH] Manual Trajectory - Add Z position lock logic and log
complete trajectory
---
.../FlightTaskManualPositionSmoothVel.cpp | 37 +++++++++++++++----
.../FlightTaskManualPositionSmoothVel.hpp | 3 ++
2 files changed, 33 insertions(+), 7 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 5e0109ee5f..6da1b3e8d4 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -76,8 +76,10 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes)
}
_position_lock_xy_active = false;
+ _position_lock_z_active = false;
_position_setpoint_xy_locked(0) = NAN;
_position_setpoint_xy_locked(1) = NAN;
+ _position_setpoint_z_locked = NAN;
}
void FlightTaskManualPositionSmoothVel::_updateSetpoints()
@@ -140,6 +142,16 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_position_lock_xy_active = false;
}
+ if (fabsf(_sticks_expo(2)) > FLT_EPSILON) {
+ if (_position_lock_z_active) {
+ _smoothing[2].setCurrentVelocity(_velocity_setpoint_feedback(
+ 0)); // Start the trajectory at the current velocity setpoint
+ _position_setpoint_z_locked = NAN;
+ }
+
+ _position_lock_z_active = false;
+ }
+
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
@@ -157,27 +169,38 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
}
}
- Vector3f pos_sp_smooth;
- Vector3f accel_sp_smooth;
+ if (!_position_lock_xy_active) {
+ _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) {
- 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));
+ _smoothing[i].integrate(_acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i));
_velocity_setpoint(i) = _vel_sp_smooth(i); // Feedforward
+ _jerk_setpoint(i) = _smoothing[i].getCurrentJerk();
}
// Check for position lock transition
if (Vector2f(_vel_sp_smooth).length() < 0.01f &&
- Vector2f(accel_sp_smooth).length() < .2f &&
+ Vector2f(_acceleration_setpoint).length() < .2f &&
sticks_expo_xy.length() <= FLT_EPSILON) {
_position_setpoint_xy_locked(0) = pos_sp_smooth(0);
_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) {
+ _position_setpoint_z_locked = pos_sp_smooth(2);
+ _position_lock_z_active = true;
+ }
_position_setpoint(0) = _position_setpoint_xy_locked(0);
_position_setpoint(1) = _position_setpoint_xy_locked(1);
+ _position_setpoint(2) = _position_setpoint_z_locked;
}
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index 945c76fe45..cd8e8e9f18 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -69,6 +69,9 @@ private:
VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
matrix::Vector3f _vel_sp_smooth;
bool _position_lock_xy_active{false};
+ bool _position_lock_z_active{false};
matrix::Vector2f _position_setpoint_xy_locked;
+ float _position_setpoint_z_locked;
+
uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */
};
--
GitLab