diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
index 1bfeda50e2a09c1eb6e0b0a7b604f4763c946957..20c55cd636cd55b23299c2d6e4e4af669c8caa10 100644
--- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
+++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp
@@ -60,8 +60,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
_smoothing[1].setMaxAccel(MPC_ACC_HOR_MAX.get());
_smoothing[0].setMaxVel(_constraints.speed_xy);
_smoothing[1].setMaxVel(_constraints.speed_xy);
- _smoothing[0].setDt(_deltatime);
- _smoothing[1].setDt(_deltatime);
Vector2f vel_xy = Vector2f(&_velocity(0));
float jerk = _jerk_max.get();
@@ -78,7 +76,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints()
for (int i = 0; i < 2; ++i) {
_smoothing[i].setMaxJerk(jerk);
- _smoothing[i].updateDurations(_velocity_setpoint(i));
+ _smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i));
}
VelocitySmoothing::timeSynchronization(_smoothing, 2);
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
index 84d03b06ed5ecacffe6ce115d5fb526070527942..7b0b3840884951d9186815caec6320ff823628bd 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
@@ -92,7 +92,7 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
float a = -max_jerk;
float b = max_jerk * T123 - accel_prev;
float delta = T123 * T123 * max_jerk * max_jerk + 2.f * T123 * accel_prev * max_jerk - accel_prev * accel_prev
- + 4.f * max_jerk * (vel_prev - vel_setpoint);
+ + 4.f * max_jerk * (vel_prev - vel_setpoint);
float sqrt_delta = sqrtf(delta);
float denominator_inv = 1.f / (2.f * a);
@@ -106,9 +106,11 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev,
float T13_minus = T1_minus + T3_minus;
float T1 = 0.f;
+
if (T13_plus > T123) {
T1 = T1_minus;
- } else if (T13_minus > T123){
+
+ } else if (T13_minus > T123) {
T1 = T1_plus;
}
@@ -161,18 +163,26 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
pos_out = _dt / 3.f * (vel_out + accel_prev * _dt * 0.5f + 2.f * vel_prev) + _pos;
}
-void VelocitySmoothing::updateDurations(float vel_setpoint, float T123)
+void VelocitySmoothing::updateDurations(float dt, float vel_setpoint)
+{
+ _vel_sp = vel_setpoint;
+ _dt = dt;
+ updateDurations();
+}
+
+void VelocitySmoothing::updateDurations(float T123)
{
float T1, T2, T3;
/* Depending of the direction, start accelerating positively or negatively */
- _max_jerk_T1 = (vel_setpoint - _vel > 0.f) ? _max_jerk : -_max_jerk;
+ _max_jerk_T1 = (_vel_sp - _vel > 0.f) ? _max_jerk : -_max_jerk;
// compute increasing acceleration time
if (PX4_ISFINITE(T123)) {
- T1 = computeT1(T123, _accel, _vel, vel_setpoint, _max_jerk_T1);
+ T1 = computeT1(T123, _accel, _vel, _vel_sp, _max_jerk_T1);
+
} else {
- T1 = computeT1(_accel, _vel, vel_setpoint, _max_jerk_T1);
+ T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1);
}
/* Force T1/2/3 to zero if smaller than an epoch to avoid chattering */
@@ -190,8 +200,9 @@ void VelocitySmoothing::updateDurations(float vel_setpoint, float T123)
// compute constant acceleration time
if (PX4_ISFINITE(T123)) {
T2 = computeT2(T123, T1, T3);
+
} else {
- T2 = computeT2(T1, T3, _accel, _vel, vel_setpoint, _max_jerk_T1);
+ T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1);
}
if (T2 < _dt) {
@@ -201,11 +212,10 @@ void VelocitySmoothing::updateDurations(float vel_setpoint, float T123)
_T1 = T1;
_T2 = T2;
_T3 = T3;
- _vel_sp = vel_setpoint;
}
void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
- float &pos_setpoint_smooth)
+ float &pos_setpoint_smooth)
{
/* Integrate the trajectory */
float accel_new, vel_new, pos_new;
@@ -240,22 +250,24 @@ void VelocitySmoothing::integrate(float pos, float &vel_setpoint_smooth,
pos_setpoint_smooth = _pos;
}
-void VelocitySmoothing::timeSynchronization(VelocitySmoothing traj[3], int n_traj)
+void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj)
{
float desired_time = 0.f;
int longest_traj_index = 0;
for (int i = 0; i < n_traj; i++) {
const float T123 = traj[i].getTotalTime();
+
if (T123 > desired_time) {
desired_time = T123;
longest_traj_index = i;
}
}
+
if (desired_time > FLT_EPSILON) {
for (int i = 0; i < n_traj; i++) {
if (i != longest_traj_index) {
- traj[i].updateDurations(traj[i].getVelSp(), desired_time);
+ traj[i].updateDurations(desired_time);
}
}
}
diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
index 6d78b6d5e98b4cd45748bcce5679502dc48ad9d4..5a376f51222ff377b9bfd45af535944317afab1f 100644
--- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
@@ -68,12 +68,12 @@ public:
void reset(float accel, float vel, float pos);
/**
- * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle.
+ * Compute T1, T2, T3 depending on the current state and velocity setpoint. This should be called on every cycle
+ * and before integrate().
+ * @param dt delta time between last updateDurations() call and now [s]
* @param vel_setpoint velocity setpoint input
- * @param T123 optional parameter. If set, the total trajectory time will be T123, if not,
- * the algorithm optimizes for time.
*/
- void updateDurations(float vel_setpoint, float T123 = NAN);
+ void updateDurations(float dt, float vel_setpoint);
/**
* Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
@@ -93,22 +93,25 @@ public:
float getMaxVel() const { return _max_vel; }
void setMaxVel(float max_vel) { _max_vel = max_vel; }
- /* Other getters and setters */
- float getTotalTime() const {return _T1 + _T2 + _T3; }
- float getVelSp() const {return _vel_sp; }
-
- void setDt(float dt) {_dt = dt; } // delta time between last update() call and now [s]
-
/**
* Synchronize several trajectories to have the same total time. This is required to generate
* straight lines.
* The resulting total time is the one of the longest trajectory.
- * @param traj[3] a table of VelocitySmoothing objects
- * n_traj the number of trajectories to be synchronized
+ * @param traj an array of VelocitySmoothing objects
+ * @param n_traj the number of trajectories to be synchronized
*/
- static void timeSynchronization(VelocitySmoothing traj[3], int n_traj);
+ static void timeSynchronization(VelocitySmoothing *traj, int n_traj);
private:
+ float getTotalTime() const { return _T1 + _T2 + _T3; }
+ float getVelSp() const { return _vel_sp; }
+
+ /**
+ * Compute T1, T2, T3 depending on the current state and velocity setpoint.
+ * @param T123 optional parameter. If set, the total trajectory time will be T123, if not,
+ * the algorithm optimizes for time.
+ */
+ void updateDurations(float T123 = NAN);
/**
* Compute increasing acceleration time
*/
@@ -136,6 +139,10 @@ private:
inline void integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
float &accel_out, float &vel_out, float &pos_out);
+ /* Inputs */
+ float _vel_sp;
+ float _dt = 0.f;
+
/* Constraints */
float _max_jerk = 22.f;
float _max_accel = 8.f;
@@ -150,12 +157,9 @@ private:
float _max_jerk_T1 = 0.f; ///< jerk during phase T1 (with correct sign)
/* Duration of each phase */
- float _T1 = 0.f; // Increasing acceleration
- float _T2 = 0.f; // Constant acceleration
- float _T3 = 0.f; // Decreasing acceleration
-
- float _vel_sp;
- float _dt = 0.f;
+ float _T1 = 0.f; ///< Increasing acceleration [s]
+ float _T2 = 0.f; ///< Constant acceleration [s]
+ float _T3 = 0.f; ///< Decreasing acceleration [s]
static constexpr float max_pos_err = 1.f; ///< maximum position error (if above, the position setpoint is locked)
};