From 8490266be8a3a036010d049cc5811cf17f9b5ccd Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 12 Sep 2019 17:08:48 +0200 Subject: [PATCH] VelocitySmoothing - (Re)set _state_init every time _state is (re)set --- src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp | 2 ++ src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp | 6 +++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp index a2d34479f3..ec85a9d6df 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp @@ -48,6 +48,8 @@ void VelocitySmoothing::reset(float accel, float vel, float pos) _state.a = accel; _state.v = vel; _state.x = pos; + + _state_init = _state; } float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) diff --git a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp index 1930b3aaea..ba817a8b98 100644 --- a/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp +++ b/src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp @@ -100,11 +100,11 @@ public: void setMaxVel(float max_vel) { _max_vel = max_vel; } float getCurrentJerk() const { return _state.j; } - void setCurrentAcceleration(const float accel) { _state.a = accel; } + void setCurrentAcceleration(const float accel) { _state.a = _state_init.a = accel; } float getCurrentAcceleration() const { return _state.a; } - void setCurrentVelocity(const float vel) { _state.v = vel; } + void setCurrentVelocity(const float vel) { _state.v = _state_init.v = vel; } float getCurrentVelocity() const { return _state.v; } - void setCurrentPosition(const float pos) { _state.x = pos; } + void setCurrentPosition(const float pos) { _state.x = _state_init.x = pos; } float getCurrentPosition() const { return _state.x; } float getVelSp() const { return _vel_sp; }