From 6a7ce651bc0fc8df73fe087e432fedec1387a1e1 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Wed, 10 Oct 2018 15:59:09 +0200
Subject: [PATCH] Trajectory - Add position lock-unlock logic and proper
initialization from controller feedback
---
src/lib/FlightTasks/FlightTasks.hpp | 2 +
.../tasks/FlightTask/FlightTask.hpp | 5 ++
.../FlightTaskManualPositionSmoothVel.cpp | 50 ++++++++++++++++---
.../FlightTaskManualPositionSmoothVel.hpp | 2 +
.../tasks/Utility/VelocitySmoothing.cpp | 10 +---
.../tasks/Utility/VelocitySmoothing.hpp | 7 ++-
.../mc_pos_control/mc_pos_control_main.cpp | 1 +
7 files changed, 59 insertions(+), 18 deletions(-)
diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 5eb228aa7b..3a07d25ae3 100644
--- a/src/lib/FlightTasks/FlightTasks.hpp
+++ b/src/lib/FlightTasks/FlightTasks.hpp
@@ -136,6 +136,8 @@ public:
*/
void reActivate();
+ void updateVelocityControllerIO(const matrix::Vector3f &vel_sp, const matrix::Vector3f &thrust_sp) {_current_task.task->updateVelocityControllerIO(vel_sp, thrust_sp); }
+
private:
/**
diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
index d2d843e226..ca98239ef5 100644
--- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
@@ -147,6 +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; }
+
protected:
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
@@ -197,6 +199,9 @@ protected:
float _yaw_setpoint;
float _yawspeed_setpoint;
+ matrix::Vector3f _velocity_setpoint_feedback;
+ matrix::Vector3f _thrust_setpoint_feedback;
+
/**
* Vehicle constraints.
* The constraints can vary with tasks.
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index e18d28a9bc..5a2b215e88 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -47,6 +47,10 @@ bool FlightTaskManualPositionSmoothVel::activate()
_smoothing[i].reset(0.f, _velocity(i), _position(i));
}
+ _position_lock_xy_active = false;
+ _position_setpoint_xy_locked(0) = NAN;
+ _position_setpoint_xy_locked(1) = NAN;
+
return ret;
}
@@ -81,9 +85,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
if (_jerk_min.get() > FLT_EPSILON) {
if (vel_xy_sp.length() < FLT_EPSILON) { // Brake
- jerk_xy = _jerk_min.get() + (_jerk_max.get() - _jerk_min.get());
-
- } else if (vel_xy_sp.dot(vel_xy_sp_smooth) < -FLT_EPSILON) { // Reverse
jerk_xy = _jerk_max.get();
} else {
@@ -94,6 +95,23 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
jerk[0] = jerk_xy;
jerk[1] = jerk_xy;
+ /* Check for position unlock
+ * During a position lock -> position unlock transition, we have to make sure that the velocity setpoint
+ * is continuous. We know that the output of the position loop (part of the velocity setpoint) will suddenly become null
+ * and only the feedforward (generated by this flight task) will remain. This is why the previous input of the velocity controller
+ * 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[1].setCurrentVelocity(_velocity_setpoint_feedback(1));
+ _position_setpoint_xy_locked(0) = NAN;
+ _position_setpoint_xy_locked(1) = NAN;
+ }
+ _position_lock_xy_active = false;
+ }
+
for (int i = 0; i < 3; ++i) {
_smoothing[i].setMaxJerk(jerk[i]);
_smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
@@ -101,11 +119,27 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only
+ Vector3f pos_sp_smooth;
+ Vector3f accel_sp_smooth;
+
for (int i = 0; i < 3; ++i) {
- float smoothed_position_setpoint;
- _smoothing[i].integrate(_position(i), _vel_sp_smooth(i), smoothed_position_setpoint);
- //printf("\nTraj %d\tNew total time = %.3f", i, (double)_smoothing[i].getTotalTime());
- _position_setpoint(i) = smoothed_position_setpoint;
- _velocity_setpoint(i) = _vel_sp_smooth(i);
+ _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 &&
+ 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;
+ }
+
+ _position_setpoint(0) = _position_setpoint_xy_locked(0);
+ _position_setpoint(1) = _position_setpoint_xy_locked(1);
}
diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
index 5b02552c36..b224ea65c5 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp
@@ -65,4 +65,6 @@ private:
VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions
matrix::Vector3f _vel_sp_smooth;
+ bool _position_lock_xy_active{false};
+ matrix::Vector2f _position_setpoint_xy_locked;
};
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index 7b0b384088..81d6018cd9 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -214,7 +214,7 @@ void VelocitySmoothing::updateDurations(float T123)
_T3 = T3;
}
-void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
+void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
float &pos_setpoint_smooth)
{
/* Integrate the trajectory */
@@ -238,14 +238,8 @@ void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
_accel = accel_new;
_vel = vel_new;
- /* Lock the position setpoint if the error is bigger than some value */
- float x_err = pos_new - pos;
-
- if (fabsf(x_err) <= max_pos_err) {
- _pos = pos_new;
- } // else: keep last position
-
/* set output variables */
+ accel_setpoint_smooth = _accel;
vel_setpoint_smooth = _vel;
pos_setpoint_smooth = _pos;
}
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index 5a376f5122..9fbf777f5b 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -77,11 +77,11 @@ public:
/**
* Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
- * @param pos Current vehicle's position (used for position error clamping)
+ * @param acc_setpoint_smooth returned smoothed acceleration setpoint
* @param vel_setpoint_smooth returned smoothed velocity setpoint
* @param pos_setpoint_smooth returned smoothed position setpoint
*/
- void integrate(float pos, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
+ void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
/* Get / Set constraints (constraints can be updated at any time) */
float getMaxJerk() const { return _max_jerk; }
@@ -93,6 +93,9 @@ public:
float getMaxVel() const { return _max_vel; }
void setMaxVel(float max_vel) { _max_vel = max_vel; }
+ void setCurrentVelocity(const float vel) { _vel = vel; }
+ void setCurrentPosition(const float pos) { _pos = pos; }
+
/**
* Synchronize several trajectories to have the same total time. This is required to generate
* straight lines.
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index bc331fa25e..9f8251e4a1 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -768,6 +768,7 @@ MulticopterPositionControl::run()
// Publish local position setpoint (for logging only) and attitude setpoint (for attitude controller).
publish_local_pos_sp(local_pos_sp);
+ _flight_tasks.updateVelocityControllerIO(_control.getVelSp(), thr_sp); // Inform FlightTask about the input and output of the velocity controller
// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
_att_sp = ControlMath::thrustToAttitude(thr_sp, _control.getYawSetpoint());
--
GitLab