diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index b6a3697a7e..878ac17a55 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -349,13 +349,13 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() Vector2f position_xy(_position); Vector2f vel_traj_xy(_trajectory[0].getCurrentVelocity(), _trajectory[1].getCurrentVelocity()); Vector2f drone_to_trajectory_xy(position_trajectory_xy - position_xy); - float position_error = drone_to_trajectory_xy.length(); + //float position_error = drone_to_trajectory_xy.length(); - float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f); + //float time_stretch = 1.f - math::constrain(position_error * 0.5f, 0.f, 1.f); // Don't stretch time if the drone is ahead of the position setpoint if (drone_to_trajectory_xy.dot(vel_traj_xy) < 0.f) { - time_stretch = 1.f; + //time_stretch = 1.f; } Vector3f jerk_sp_smooth; @@ -364,23 +364,16 @@ void FlightTaskAutoLineSmoothVel::_generateTrajectory() Vector3f pos_sp_smooth; for (int i = 0; i < 3; ++i) { - _trajectory[i].integrate(_deltatime, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i)); + // TODO: fix time stretch + //_trajectory[i].updateTraj(_time_stamp_current, time_stretch, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i)); + _trajectory[i].updateTraj(_time, accel_sp_smooth(i), vel_sp_smooth(i), pos_sp_smooth(i)); jerk_sp_smooth(i) = _trajectory[i].getCurrentJerk(); } _updateTrajConstraints(); - // If the acceleration and velocities are small and that we want to stop, reduce the amplitude of the jerk signal - // to help the optimizer to converge towards zero - if (Vector2f(_velocity_setpoint).length() < (0.01f * _param_mpc_xy_traj_p.get()) - && Vector2f(accel_sp_smooth).length() < 0.2f - && Vector2f(vel_sp_smooth).length() < 0.1f) { - _trajectory[0].setMaxJerk(1.f); - _trajectory[1].setMaxJerk(1.f); - } - for (int i = 0; i < 3; ++i) { - _trajectory[i].updateDurations(_deltatime, _velocity_setpoint(i)); + _trajectory[i].updateDurations(_time, _velocity_setpoint(i)); } VelocitySmoothing::timeSynchronization(_trajectory, 2); // Synchronize x and y only diff --git a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp index b2e5ec473b..28afa170b9 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.cpp @@ -155,7 +155,8 @@ void FlightTaskManualAltitudeSmoothVel::_updateSetpoints() float pos_sp_smooth; - _smoothing.integrate(_acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth); + // TODO: move before updateDurations + _smoothing.updateTraj(_time_stamp_current, _acceleration_setpoint(2), _vel_sp_smooth, pos_sp_smooth); _velocity_setpoint(2) = _vel_sp_smooth; // Feedforward _jerk_setpoint(2) = _smoothing.getCurrentJerk(); diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index d73b012ee6..3d42e8f959 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -141,6 +141,13 @@ void FlightTaskManualPositionSmoothVel::_checkEkfResetCounters() void FlightTaskManualPositionSmoothVel::_updateSetpoints() { + Vector3f pos_sp_smooth; + + for (int i = 0; i < 3; ++i) { + _smoothing[i].updateTraj(_time_stamp_current, _acceleration_setpoint(i), _vel_sp_smooth(i), pos_sp_smooth(i)); + _jerk_setpoint(i) = _smoothing[i].getCurrentJerk(); + } + /* Get yaw setpont, un-smoothed position setpoints.*/ FlightTaskManualPosition::_updateSetpoints(); @@ -194,22 +201,9 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _position_lock_z_active = false; } - // During position lock, lower jerk to help the optimizer - // to converge to 0 acceleration and velocity - if (_position_lock_xy_active) { - jerk[0] = 1.f; - jerk[1] = 1.f; - - } else { - jerk[0] = _param_mpc_jerk_max.get(); - jerk[1] = _param_mpc_jerk_max.get(); - } - - jerk[2] = _position_lock_z_active ? 1.f : _param_mpc_jerk_max.get(); - for (int i = 0; i < 3; ++i) { _smoothing[i].setMaxJerk(jerk[i]); - _smoothing[i].updateDurations(_deltatime, _velocity_setpoint(i)); + _smoothing[i].updateDurations(_time_stamp_current, _velocity_setpoint(i)); } VelocitySmoothing::timeSynchronization(_smoothing, 2); // Synchronize x and y only @@ -223,14 +217,6 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() _smoothing[2].setCurrentPosition(_position(2)); } - Vector3f pos_sp_smooth; - - for (int i = 0; i < 3; ++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.1f && Vector2f(_acceleration_setpoint).length() < .2f && @@ -276,6 +262,7 @@ void FlightTaskManualPositionSmoothVel::_updateSetpoints() } } + _velocity_setpoint = _vel_sp_smooth; // Feedforward _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/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index 0db669f24c..efae62a3b5 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,33 +45,31 @@ VelocitySmoothing::VelocitySmoothing(float initial_accel, float initial_vel, flo void VelocitySmoothing::reset(float accel, float vel, float pos) { - _jerk = 0.f; - _accel = accel; - _vel = vel; - _pos = pos; + _state.j = 0.f; + _state.a = accel; + _state.v = vel; + _state.x = pos; } -float VelocitySmoothing::saturateT1ForAccel(float accel_prev, float max_jerk, float T1) +float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) { /* Check maximum acceleration, saturate and recompute T1 if needed */ - float accel_T1 = accel_prev + max_jerk * T1; + float accel_T1 = a0 + j_max * T1; float T1_new = T1; - if (accel_T1 > _max_accel) { - T1_new = (_max_accel - accel_prev) / max_jerk; + if (accel_T1 > a_max) { + T1_new = (a_max - a0) / j_max; - } else if (accel_T1 < -_max_accel) { - T1_new = (-_max_accel - accel_prev) / max_jerk; + } else if (accel_T1 < -a_max) { + T1_new = (-a_max - a0) / j_max; } return T1_new; } -float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk) +float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) { - float b = 2.f * accel_prev / max_jerk; - float c = vel_prev / max_jerk + accel_prev * accel_prev / (2.f * max_jerk * max_jerk) - vel_setpoint / max_jerk; - float delta = b * b - 4.f * c; + float delta = 2.f * a0 * a0 + 4.f * j_max * v3; if (delta < 0.f) { // Solution is not real @@ -79,11 +77,11 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s } float sqrt_delta = sqrtf(delta); - float T1_plus = (-b + sqrt_delta) * 0.5f; - float T1_minus = (-b - sqrt_delta) * 0.5f; + float T1_plus = (-a0 + 0.5f * sqrt_delta) / j_max; + float T1_minus = (-a0 - 0.5f * sqrt_delta) / j_max; - float T3_plus = accel_prev / max_jerk + T1_plus; - float T3_minus = accel_prev / max_jerk + T1_minus; + float T3_plus = a0 / j_max + T1_plus; + float T3_minus = a0 / j_max + T1_minus; float T1 = 0.f; @@ -94,21 +92,16 @@ float VelocitySmoothing::computeT1(float accel_prev, float vel_prev, float vel_s T1 = T1_minus; } - T1 = saturateT1ForAccel(accel_prev, max_jerk, T1); - - if (T1 < _dt) { - T1 = 0.f; - } + T1 = saturateT1ForAccel(a0, j_max, T1, a_max); return math::max(T1, 0.f); } -float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk) +float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max) { - 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); + float a = -j_max; + float b = j_max * T123 - a0; + float delta = T123 * T123 * j_max * j_max + 2.f * T123 *a0 * j_max - a0 * a0 - 4.f * j_max * v3; if (delta < 0.f) { // Solution is not real @@ -120,8 +113,8 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev, float T1_plus = math::max((-b + sqrt_delta) * denominator_inv, 0.f); float T1_minus = math::max((-b - sqrt_delta) * denominator_inv, 0.f); - float T3_plus = accel_prev / max_jerk + T1_plus; - float T3_minus = accel_prev / max_jerk + T1_minus; + float T3_plus = a0 / j_max + T1_plus; + float T3_minus = a0 / j_max + T1_minus; float T13_plus = T1_plus + T3_plus; float T13_minus = T1_minus + T3_minus; @@ -135,31 +128,20 @@ float VelocitySmoothing::computeT1(float T123, float accel_prev, float vel_prev, T1 = T1_plus; } - T1 = saturateT1ForAccel(accel_prev, max_jerk, T1); - - if (T1 < _dt) { - T1 = 0.f; - } + T1 = saturateT1ForAccel(a0, j_max, T1, a_max); return T1; } -float VelocitySmoothing::computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint, - float max_jerk) +float VelocitySmoothing::computeT2(float T1, float T3, float a0 , float v3, float j_max) { - float f = accel_prev * T1 + max_jerk * T1 * T1 * 0.5f + vel_prev + accel_prev * T3 + max_jerk * T1 * T3 - - max_jerk * T3 * T3 * 0.5f; float T2 = 0.f; - float den = accel_prev + max_jerk * T1; + float den = a0 + j_max * T1; if (math::abs_t(den) > FLT_EPSILON) { - T2 = (vel_setpoint - f) / den; - } - - if (T2 < _dt) { - T2 = 0.f; + T2 = (-0.5f *T1 * T1 * j_max - T1 * T3*j_max - T1 * a0 + 0.5f * T3 * T3 * j_max - T3 * a0 + v3) / den; } return math::max(T2, 0.f); @@ -171,56 +153,51 @@ float VelocitySmoothing::computeT2(float T123, float T1, float T3) return math::max(T2, 0.f); } -float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk) +float VelocitySmoothing::computeT3(float T1, float a0, float j_max) { - float T3 = accel_prev / max_jerk + T1; - - if (T1 < FLT_EPSILON && T3 < _dt && T3 > 0.f) { - T3 = _dt; - _max_jerk_T1 = accel_prev / T3; - } + float T3 = a0 / j_max + T1; return math::max(T3, 0.f); } -void VelocitySmoothing::integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev, - float &accel_out, float &vel_out, float &pos_out) -{ - accel_out = jerk * dt + accel_prev; - - vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev; - - pos_out = dt / 3.f * (vel_out + accel_prev * dt * 0.5f + 2.f * vel_prev) + _pos; -} - -void VelocitySmoothing::updateDurations(float dt, float vel_setpoint) +void VelocitySmoothing::updateDurations(float t_now, float vel_setpoint) { _vel_sp = math::constrain(vel_setpoint, -_max_vel, _max_vel); - _dt = math::max(dt, FLT_EPSILON); - updateDurations(); + _t0 = t_now; + _state_init = _state; + + /* Depending of the direction, start accelerating positively or negatively */ + _direction = math::sign(_vel_sp - _state.v); + + if (_direction != 0) { + updateDurations(); + + } else { + _T1 = _T2 = _T3 = 0.f; + } } void VelocitySmoothing::updateDurations(float T123) { float T1, T2, T3; - /* Depending of the direction, start accelerating positively or negatively */ - _max_jerk_T1 = (_vel_sp - _vel > 0.f) ? _max_jerk : -_max_jerk; + float jerk_max_T1 = _direction * _max_jerk; + float delta_v = _vel_sp - _state.v; // compute increasing acceleration time if (T123 < 0.f) { - T1 = computeT1(_accel, _vel, _vel_sp, _max_jerk_T1); + T1 = computeT1(_state.a, delta_v, jerk_max_T1, _max_accel); } else { - T1 = computeT1(T123, _accel, _vel, _vel_sp, _max_jerk_T1); + T1 = computeT1(T123, _state.a, delta_v, jerk_max_T1, _max_accel); } // compute decreasing acceleration time - T3 = computeT3(T1, _accel, _max_jerk_T1); + T3 = computeT3(T1, _state.a, jerk_max_T1); // compute constant acceleration time if (T123 < 0.f) { - T2 = computeT2(T1, T3, _accel, _vel, _vel_sp, _max_jerk_T1); + T2 = computeT2(T1, T3, _state.a, delta_v, jerk_max_T1); } else { T2 = computeT2(T123, T1, T3); @@ -231,52 +208,57 @@ void VelocitySmoothing::updateDurations(float T123) _T3 = T3; } -void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, - float &pos_setpoint_smooth) +Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) { - integrate(_dt, 1.f, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth); + Trajectory traj; + float jt = d * j; + float t2 = t * t; + float t3 = t2 * t; + + traj.j = jt; + traj.a = a0 + jt * t; + traj.v = v0 + a0 * t + 0.5f * jt * t2; + traj.x = x0 + v0 * t + 0.5f * a0 * t2 + 1.f / 6.f * jt * t3; + + return traj; } -void VelocitySmoothing::integrate(float dt, float integration_scale_factor, float &accel_setpoint_smooth, - float &vel_setpoint_smooth, - float &pos_setpoint_smooth) +void VelocitySmoothing::updateTraj(float t_now, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth) { - /* Apply correct jerk (min, max or zero) */ - if (_T1 > FLT_EPSILON) { - _jerk = _max_jerk_T1; + const float t = t_now - _t0; - if (_T1 < dt && dt > _dt) { - // _T1 was supposed to be _dt, however, now, dt is bigger than _dt. We have to reduce the jerk to avoid an acceleration overshoot. - _jerk *= _dt / dt; // Keep the same area _dt * _jerk = dt * jerk_new - } + if (t <= _T1) { + float t1 = t; + _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); - } else if (_T2 > FLT_EPSILON) { - _jerk = 0.f; + } else if (t <= _T1 + _T2) { + float t1 = _T1; + float t2 = t - _T1; + _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); + _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); - } else if (_T3 > FLT_EPSILON) { - _jerk = -_max_jerk_T1; - - if (_T3 < dt && dt > _dt) { - // Same as for _T1 < dt above - _jerk *= _dt / dt; - } + } else if (t <= _T1 + _T2 + _T3) { + float t1 = _T1; + float t2 = _T2; + float t3 = t - _T1 - _T2; + _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); + _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); + _state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction); } else { - _jerk = 0.f; + float t1 = _T1; + float t2 = _T2; + float t3 = _T3; + float t4 = t - _T1 - _T2 - _T3; + _state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction); + _state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f); + _state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction); + _state = evaluatePoly(0.f, 0.f, _state.v, _state.x, t4, 0.f); } - /* Integrate the trajectory */ - float accel_new, vel_new, pos_new; - integrateT(dt * integration_scale_factor, _jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new); - - _accel = accel_new; - _vel = vel_new; - _pos = pos_new; - - /* set output variables */ - accel_setpoint_smooth = _accel; - vel_setpoint_smooth = _vel; - pos_setpoint_smooth = _pos; + accel_setpoint_smooth = _state.a; + vel_setpoint_smooth = _state.v; + pos_setpoint_smooth = _state.x; } void VelocitySmoothing::timeSynchronization(VelocitySmoothing *traj, int n_traj) diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp index 7e744384c1..abcafa7465 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp @@ -33,6 +33,13 @@ #pragma once +struct Trajectory { + float j; //< jerk + float a; //< acceleration + float v; //< velocity + float x; //< position +}; + /** * @class VelocitySmoothing * @@ -67,11 +74,11 @@ public: /** * 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] + * and before evaluateTraj(). * @param vel_setpoint velocity setpoint input + * @param t current time */ - void updateDurations(float dt, float vel_setpoint); + void updateDurations(float t_now, float vel_setpoint); /** * Generate the trajectory (acceleration, velocity and position) by integrating the current jerk @@ -81,27 +88,27 @@ public: * @param vel_setpoint_smooth returned smoothed velocity setpoint * @param pos_setpoint_smooth returned smoothed position setpoint */ - void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth); - void integrate(float dt, float integration_scale_factor, float &accel_setpoint_smooth, float &vel_setpoint_smooth, - float &pos_setpoint_smooth); + void updateTraj(float t_now, 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; } void setMaxJerk(float max_jerk) { _max_jerk = max_jerk; } + + float getMaxAccel() const { return _max_accel; } void setMaxAccel(float max_accel) { _max_accel = max_accel; } float getMaxVel() const { return _max_vel; } void setMaxVel(float max_vel) { _max_vel = max_vel; } - float getCurrentJerk() const { return _jerk; } - void setCurrentAcceleration(const float accel) { _accel = accel; } - float getCurrentAcceleration() const { return _accel; } - void setCurrentVelocity(const float vel) { _vel = vel; } - float getCurrentVelocity() const { return _vel; } - void setCurrentPosition(const float pos) { _pos = pos; } - float getCurrentPosition() const { return _pos; } + float getCurrentJerk() const { return _state.j; } + void setCurrentAcceleration(const float accel) { _state.a = accel; } + float getCurrentAcceleration() const { return _state.a; } + void setCurrentVelocity(const float vel) { _state.v = vel; } + float getCurrentVelocity() const { return _state.v; } + void setCurrentPosition(const float pos) { _state.x = pos; } + float getCurrentPosition() const { return _state.x; } /** * Synchronize several trajectories to have the same total time. This is required to generate @@ -129,16 +136,16 @@ private: /** * Compute increasing acceleration time */ - inline float computeT1(float accel_prev, float vel_prev, float vel_setpoint, float max_jerk); + inline float computeT1(float a0, float v3, float j_max, float a_max); /** * Compute increasing acceleration time using total time constraint */ - inline float computeT1(float T123, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk); - inline float saturateT1ForAccel(float accel_prev, float max_jerk, float T1); + inline float computeT1(float T123, float a0, float v3, float j_max, float a_max); + inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max); /** * Compute constant acceleration time */ - inline float computeT2(float T1, float T3, float accel_prev, float vel_prev, float vel_setpoint, float max_jerk); + inline float computeT2(float T1, float T3, float a0, float v3, float j_max); /** * Compute constant acceleration time using total time constraint */ @@ -146,17 +153,22 @@ private: /** * Compute decreasing acceleration time */ - inline float computeT3(float T1, float accel_prev, float max_jerk); + inline float computeT3(float T1, float a0, float j_max); /** - * Integrate the jerk, acceleration and velocity to get the new setpoints and states. + * Compute the jerk, acceleration, velocity and position + * of a jerk-driven polynomial trajectory at a given time t + * @param j jerk + * @param a0 initial acceleration at t = 0 + * @param v0 initial velocity + * @param x0 initial postion + * @param t current time + * @param d direction */ - inline void integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev, - float &accel_out, float &vel_out, float &pos_out); + inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d); - /* Inputs */ + /* Input */ float _vel_sp{0.0f}; - float _dt = 1.f; /* Constraints */ float _max_jerk = 22.f; @@ -164,17 +176,16 @@ private: float _max_vel = 6.f; /* State (previous setpoints) */ - float _jerk = 0.f; - float _accel = 0.f; - float _vel = 0.f; - float _pos = 0.f; + Trajectory _state{}; + int _direction{0}; - float _max_jerk_T1 = 0.f; ///< jerk during phase T1 (with correct sign) + /* Initial conditions */ + Trajectory _state_init{}; /* Duration of each phase */ 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) + float _t0 = 0.f; ///< Starting time }; diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp index 41e85b91e6..d3bc0831d6 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothingTest.cpp @@ -48,7 +48,7 @@ class VelocitySmoothingTest : public ::testing::Test public: void setConstraints(float j_max, float a_max, float v_max); void setInitialConditions(Vector3f acc, Vector3f vel, Vector3f pos); - void updateTrajectories(Vector3f velocity_setpoints, float dt); + void updateTrajectories(Vector3f velocity_setpoints, float t); VelocitySmoothing _trajectories[3]; }; @@ -71,19 +71,19 @@ void VelocitySmoothingTest::setInitialConditions(Vector3f a0, Vector3f v0, Vecto } } -void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, float dt) +void VelocitySmoothingTest::updateTrajectories(Vector3f velocity_setpoints, float t) { - for (int i = 0; i < 3; i++) { - _trajectories[i].updateDurations(dt, velocity_setpoints(i)); - } - - VelocitySmoothing::timeSynchronization(_trajectories, 2); - float dummy; // We don't care about the immediate result for (int i = 0; i < 3; i++) { - _trajectories[i].integrate(dummy, dummy, dummy); + _trajectories[i].updateTraj(t, dummy, dummy, dummy); } + + for (int i = 0; i < 3; i++) { + _trajectories[i].updateDurations(t, velocity_setpoints(i)); + } + + VelocitySmoothing::timeSynchronization(_trajectories, 2); } TEST_F(VelocitySmoothingTest, testTimeSynchronization) @@ -96,16 +96,16 @@ TEST_F(VelocitySmoothingTest, testTimeSynchronization) setConstraints(j_max, a_max, v_max); // AND: A set of initial conditions - Vector3f a0(0.f, 0.f, 0.f); - Vector3f v0(0.f, 0.f, 0.f); + Vector3f a0(0.22f, 0.f, 0.22f); + Vector3f v0(2.47f, -5.59e-6f, 2.47f); Vector3f x0(0.f, 0.f, 0.f); setInitialConditions(a0, v0, x0); // WHEN: We generate trajectories (time synchronized in XY) with constant setpoints and dt - Vector3f velocity_setpoints(0.f, 1.f, 0.f); - float dt = 0.01f; - updateTrajectories(velocity_setpoints, dt); + Vector3f velocity_setpoints(-3.f, 1.f, 0.f); + updateTrajectories(velocity_setpoints, 0.f); + // THEN: The X and Y trajectories should have the same total time (= time sunchronized) EXPECT_LE(fabsf(_trajectories[0].getTotalTime() - _trajectories[1].getTotalTime()), 0.0001); @@ -114,27 +114,29 @@ TEST_F(VelocitySmoothingTest, testTimeSynchronization) TEST_F(VelocitySmoothingTest, testConstantSetpoint) { // GIVEN: A set of initial conditions (same constraints as before) - Vector3f a0(0.22f, 0.f, 0.22f); - Vector3f v0(2.47f, -5.59e-6f, 2.47f); + Vector3f a0(0.f, 0.f, 0.f); + Vector3f v0(0.f, 0.f, 0.f); Vector3f x0(0.f, 0.f, 0.f); setInitialConditions(a0, v0, x0); // WHEN: We generate trajectories with constant setpoints and dt - Vector3f velocity_setpoints(0.f, 1.f, 0.f); - float dt = 0.01f; + Vector3f velocity_setpoints(-3.f, 0.f, -1.f); // Compute the number of steps required to reach desired value // because of known numerical issues, the actual trajectory takes a // bit more time than the predicted one, this is why we have to add 14 steps // to the theoretical value. // The updateTrajectories is fist called once to compute the total time - updateTrajectories(velocity_setpoints, dt); + float t = 0.f; + const float dt = 0.01; + updateTrajectories(velocity_setpoints, t); float t123 = _trajectories[0].getTotalTime(); - int nb_steps = ceil(t123 / dt) + 14; + int nb_steps = ceil(t123 / dt); for (int i = 0; i < nb_steps; i++) { - updateTrajectories(velocity_setpoints, dt); + t += dt; + updateTrajectories(velocity_setpoints, t); } // THEN: All the trajectories should have reach their @@ -156,11 +158,13 @@ TEST_F(VelocitySmoothingTest, testZeroSetpoint) // AND: Null setpoints Vector3f velocity_setpoints(0.f, 0.f, 0.f); - float dt = 0.01f; + float t = 0.f; + const float dt = 0.01f; // WHEN: We run a few times the algorithm for (int i = 0; i < 60; i++) { - updateTrajectories(velocity_setpoints, dt); + updateTrajectories(velocity_setpoints, t); + t += dt; } // THEN: All the trajectories should still be null diff --git a/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp b/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp index de2d8f3ea3..c75c075104 100644 --- a/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/test_velocity_smoothing.cpp @@ -43,8 +43,8 @@ int main(int argc, char *argv[]) { VelocitySmoothing trajectory[3]; - float a0[3] = {0.22, 0.f, 0.22f}; - float v0[3] = {2.47f, -5.59e-6f, 2.47f}; + float a0[3] = {0.f, 0.f, 0.f}; + float v0[3] = {0.f, 0.f, 0.f}; float x0[3] = {0.f, 0.f, 0.f}; float j_max = 55.2f; @@ -59,18 +59,20 @@ int main(int argc, char *argv[]) trajectory[i].setCurrentVelocity(v0[i]); } - float dt = 0.01f; + float t = 0.f; + const float dt = 0.01f; - float velocity_setpoint[3] = {0.f, 1.f, 0.f}; + float velocity_setpoint[3] = {0.f, -1.f, 0.f}; for (int i = 0; i < 3; i++) { - trajectory[i].updateDurations(dt, velocity_setpoint[i]); + trajectory[i].updateDurations(velocity_setpoint[i], t); } - VelocitySmoothing::timeSynchronization(trajectory, 2); + //VelocitySmoothing::timeSynchronization(trajectory, 2); + t += dt; for (int i = 0; i < 3; i++) { - trajectory[i].integrate(a0[i], v0[i], x0[i]); + trajectory[i].updateTraj(t, a0[i], v0[i], x0[i]); } for (int i = 0; i < 3; i++) {