diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp index ef1a0554d2..b747dd9ab8 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.cpp @@ -41,11 +41,12 @@ using namespace matrix; void ManualVelocitySmoothingXY::reset(Vector2f accel, Vector2f vel, Vector2f pos) { for (int i = 0; i < 2; i++) { - _smoothing[i].reset(accel(i), vel(i), pos(i)); + _trajectory[i].reset(accel(i), vel(i), pos(i)); } resetPositionLock(); } + void ManualVelocitySmoothingXY::resetPositionLock() { _position_lock_active = false; @@ -69,22 +70,22 @@ void ManualVelocitySmoothingXY::update(float dt, Vector2f velocity_target) void ManualVelocitySmoothingXY::updateTrajectories(float dt) { for (int i = 0; i < 2; ++i) { - _smoothing[i].updateTraj(dt); + _trajectory[i].updateTraj(dt); - _state.j(i) = _smoothing[i].getCurrentJerk(); - _state.a(i) = _smoothing[i].getCurrentAcceleration(); - _state.v(i) = _smoothing[i].getCurrentVelocity(); - _state.x(i) = _smoothing[i].getCurrentPosition(); + _state.j(i) = _trajectory[i].getCurrentJerk(); + _state.a(i) = _trajectory[i].getCurrentAcceleration(); + _state.v(i) = _trajectory[i].getCurrentVelocity(); + _state.x(i) = _trajectory[i].getCurrentPosition(); } } void ManualVelocitySmoothingXY::updateTrajDurations(Vector2f velocity_target) { for (int i = 0; i < 2; ++i) { - _smoothing[i].updateDurations(velocity_target(i)); + _trajectory[i].updateDurations(velocity_target(i)); } - VelocitySmoothing::timeSynchronization(_smoothing, 2); + VelocitySmoothing::timeSynchronization(_trajectory, 2); } void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target) @@ -107,15 +108,15 @@ void ManualVelocitySmoothingXY::checkPositionLock(Vector2f velocity_target) } else { // Unlock position if (_position_lock_active) { - // Start the trajectory at the current velocity setpoint - _smoothing[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); - _smoothing[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); + // Start the trajectory at the current velocity setpoint + _trajectory[0].setCurrentVelocity(_velocity_setpoint_feedback(0)); + _trajectory[1].setCurrentVelocity(_velocity_setpoint_feedback(1)); _position_setpoint_locked(0) = NAN; _position_setpoint_locked(1) = NAN; } _position_lock_active = false; - _smoothing[0].setCurrentPosition(_position_estimate(0)); - _smoothing[1].setCurrentPosition(_position_estimate(1)); + _trajectory[0].setCurrentPosition(_position_estimate(0)); + _trajectory[1].setCurrentPosition(_position_estimate(1)); } } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp index 04855b0d33..2759b357dd 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingXY.hpp @@ -44,59 +44,67 @@ using matrix::Vector2f; -class ManualVelocitySmoothingXY +class ManualVelocitySmoothingXY final { public: ManualVelocitySmoothingXY() = default; - - virtual ~ManualVelocitySmoothingXY() = default; + ~ManualVelocitySmoothingXY() = default; void reset(Vector2f accel, Vector2f vel, Vector2f pos); void update(float dt, Vector2f velocity_target); void setVelSpFeedback(const Vector2f fb) { _velocity_setpoint_feedback = fb; } - void setMaxJerk(const float max_jerk) { - _smoothing[0].setMaxJerk(max_jerk); - _smoothing[1].setMaxJerk(max_jerk); + void setMaxJerk(const float max_jerk) + { + _trajectory[0].setMaxJerk(max_jerk); + _trajectory[1].setMaxJerk(max_jerk); } - float getMaxJerk() const { return _smoothing[0].getMaxJerk(); } + float getMaxJerk() const { return _trajectory[0].getMaxJerk(); } - void setMaxAccel(const float max_accel) { - _smoothing[0].setMaxAccel(max_accel); - _smoothing[1].setMaxAccel(max_accel); + void setMaxAccel(const float max_accel) + { + _trajectory[0].setMaxAccel(max_accel); + _trajectory[1].setMaxAccel(max_accel); } - float getMaxAccel() const { return _smoothing[0].getMaxAccel(); } - void setMaxVel(const float max_vel) { - _smoothing[0].setMaxVel(max_vel); - _smoothing[1].setMaxVel(max_vel); + float getMaxAccel() const { return _trajectory[0].getMaxAccel(); } + + void setMaxVel(const float max_vel) + { + _trajectory[0].setMaxVel(max_vel); + _trajectory[1].setMaxVel(max_vel); } - float getMaxVel() const { return _smoothing[0].getMaxVel(); } + float getMaxVel() const { return _trajectory[0].getMaxVel(); } Vector2f getCurrentJerk() const { return _state.j; } Vector2f getCurrentAcceleration() const { return _state.a; } - void setCurrentVelocity(const Vector2f vel) { + + void setCurrentVelocity(const Vector2f vel) + { _state.v = vel; - _smoothing[0].setCurrentVelocity(vel(0)); - _smoothing[1].setCurrentVelocity(vel(1)); + _trajectory[0].setCurrentVelocity(vel(0)); + _trajectory[1].setCurrentVelocity(vel(1)); } Vector2f getCurrentVelocity() const { return _state.v; } - void setCurrentPosition(const Vector2f pos) { + + void setCurrentPosition(const Vector2f pos) + { _state.x = pos; - _smoothing[0].setCurrentPosition(pos(0)); - _smoothing[1].setCurrentPosition(pos(1)); + _trajectory[0].setCurrentPosition(pos(0)); + _trajectory[1].setCurrentPosition(pos(1)); + // TODO: check lock/unlock in case of EKF reset } Vector2f getCurrentPosition() const { return _position_setpoint_locked; } + void setCurrentPositionEstimate(Vector2f pos) { _position_estimate = pos; } private: void resetPositionLock(); void updateTrajectories(float dt); - void updateTrajConstraints(); void checkPositionLock(Vector2f velocity_target); void updateTrajDurations(Vector2f velocity_target); - VelocitySmoothing _smoothing[2]; ///< Smoothing in x and y directions + VelocitySmoothing _trajectory[2]; ///< Trajectory in x and y directions bool _position_lock_active{false}; diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp index 187bcdd724..4b9f8c8f1e 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.cpp @@ -38,10 +38,11 @@ void ManualVelocitySmoothingZ::reset(float accel, float vel, float pos) { - _smoothing.reset(accel, vel, pos); + _trajectory.reset(accel, vel, pos); resetPositionLock(); } + void ManualVelocitySmoothingZ::resetPositionLock() { _position_lock_active = false; @@ -62,28 +63,28 @@ void ManualVelocitySmoothingZ::update(float dt, float velocity_target) checkPositionLock(velocity_target); // Update durations - _smoothing.updateDurations(velocity_target); + _trajectory.updateDurations(velocity_target); } void ManualVelocitySmoothingZ::updateTrajectories(float dt) { - _smoothing.updateTraj(dt); + _trajectory.updateTraj(dt); - _state.j = _smoothing.getCurrentJerk(); - _state.a = _smoothing.getCurrentAcceleration(); - _state.v = _smoothing.getCurrentVelocity(); - _state.x = _smoothing.getCurrentPosition(); + _state.j = _trajectory.getCurrentJerk(); + _state.a = _trajectory.getCurrentAcceleration(); + _state.v = _trajectory.getCurrentVelocity(); + _state.x = _trajectory.getCurrentPosition(); } void ManualVelocitySmoothingZ::updateTrajConstraints(float velocity_target) { if (velocity_target < 0.f) { // up - _smoothing.setMaxAccel(_max_accel_up); - _smoothing.setMaxVel(_max_vel_up); + _trajectory.setMaxAccel(_max_accel_up); + _trajectory.setMaxVel(_max_vel_up); } else { // down - _smoothing.setMaxAccel(_max_accel_down); - _smoothing.setMaxVel(_max_vel_down); + _trajectory.setMaxAccel(_max_accel_down); + _trajectory.setMaxVel(_max_vel_down); } } @@ -107,12 +108,12 @@ void ManualVelocitySmoothingZ::checkPositionLock(float velocity_target) } else { // Unlock position if (_position_lock_active) { - // Start the trajectory at the current velocity setpoint - _smoothing.setCurrentVelocity(_velocity_setpoint_feedback); + // Start the trajectory at the current velocity setpoint + _trajectory.setCurrentVelocity(_velocity_setpoint_feedback); _position_setpoint_locked = NAN; } _position_lock_active = false; - _smoothing.setCurrentPosition(_position_estimate); + _trajectory.setCurrentPosition(_position_estimate); } } diff --git a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp index 7d96803ee7..b24ee78639 100644 --- a/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ManualVelocitySmoothingZ.hpp @@ -40,44 +40,50 @@ #include "VelocitySmoothing.hpp" -class ManualVelocitySmoothingZ +class ManualVelocitySmoothingZ final { public: ManualVelocitySmoothingZ() = default; - - virtual ~ManualVelocitySmoothingZ() = default; + ~ManualVelocitySmoothingZ() = default; void reset(float accel, float vel, float pos); void update(float dt, float velocity_target); void setVelSpFeedback(const float fb) { _velocity_setpoint_feedback = fb; } - void setMaxJerk(const float max_jerk) { - _smoothing.setMaxJerk(max_jerk); + void setMaxJerk(const float max_jerk) + { + _trajectory.setMaxJerk(max_jerk); } - void setMaxAccelUp(const float max_accel_up) { + void setMaxAccelUp(const float max_accel_up) + { _max_accel_up = max_accel_up; } - void setMaxAccelDown(const float max_accel_down) { + void setMaxAccelDown(const float max_accel_down) + { _max_accel_down = max_accel_down; } - void setMaxVelUp(const float max_vel_up) { + void setMaxVelUp(const float max_vel_up) + { _max_vel_up = max_vel_up; } - void setMaxVelDown(const float max_vel_down) { + void setMaxVelDown(const float max_vel_down) + { _max_vel_down = max_vel_down; } float getCurrentJerk() const { return _state.j; } float getCurrentAcceleration() const { return _state.a; } - void setCurrentVelocity(const float vel) { + void setCurrentVelocity(const float vel) + { _state.v = vel; - _smoothing.setCurrentVelocity(vel); + _trajectory.setCurrentVelocity(vel); } float getCurrentVelocity() const { return _state.v; } - void setCurrentPosition(const float pos) { + void setCurrentPosition(const float pos) + { _state.x = pos; - _smoothing.setCurrentPosition(pos); + _trajectory.setCurrentPosition(pos); } float getCurrentPosition() const { return _position_setpoint_locked; } void setCurrentPositionEstimate(float pos) { _position_estimate = pos; } @@ -89,7 +95,7 @@ private: void checkPositionLock(float velocity_target); void updateTrajDurations(float velocity_target); - VelocitySmoothing _smoothing; ///< Smoothing in z direction + VelocitySmoothing _trajectory; ///< Trajectory in z direction bool _position_lock_active{false};