diff --git a/src/lib/FlightTasks/FlightTasks.hpp b/src/lib/FlightTasks/FlightTasks.hpp
index 5eb228aa7b074f91b80420c8b1696ce80ae06983..3a07d25ae34908c9b39202e9cf88cacc4c295666 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 d2d843e22677a062737e45cdd07f4f5f79ba19fd..ca98239ef52a77712cc8a9c9fd330fcd86a679e0 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 e18d28a9bc72f1f8820317dbdb220b985d5d06ae..5a2b215e8893988769c4e6ca4e0bc8123e09ca1a 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 5b02552c36eeaef0c051331a412f5f0ac5f5e1ae..b224ea65c5549f274ff4d17746f8b617846eddb9 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 7b0b3840884951d9186815caec6320ff823628bd..81d6018cd9d596efd29ff6026043f1e4520d894f 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 5a376f51222ff377b9bfd45af535944317afab1f..9fbf777f5bdd44dc5e64473604b71df14700d647 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 bc331fa25ecc316bf1c821edeb864c0edd7e26de..9f8251e4a18a76c81277fc703f50de7bd547f374 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());