Browse Source

VelocitySmoothing: refactor local scope t variables instead of array

sbg
Matthias Grob 5 years ago
parent
commit
270e12a4df
  1. 19
      src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp

19
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp

@ -241,22 +241,21 @@ void VelocitySmoothing::updateTraj(float dt, float time_stretch) @@ -241,22 +241,21 @@ void VelocitySmoothing::updateTraj(float dt, float time_stretch)
{
_local_time += dt * time_stretch;
float t_remain = _local_time;
float t[3];
t[0] = math::min(t_remain, _T1);
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t[0], _direction);
t_remain -= t[0];
float t1 = math::min(t_remain, _T1);
_state = evaluatePoly(_max_jerk, _state_init.a, _state_init.v, _state_init.x, t1, _direction);
t_remain -= t1;
if (t_remain > 0.f) {
t[1] = math::min(t_remain, _T2);
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t[1], 0.f);
t_remain -= t[1];
float t2 = math::min(t_remain, _T2);
_state = evaluatePoly(0.f, _state.a, _state.v, _state.x, t2, 0.f);
t_remain -= t2;
}
if (t_remain > 0.f) {
t[2] = math::min(t_remain, _T3);
_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t[2], -_direction);
t_remain -= t[2];
float t3 = math::min(t_remain, _T3);
_state = evaluatePoly(_max_jerk, _state.a, _state.v, _state.x, t3, -_direction);
t_remain -= t3;
}
if (t_remain > 0.f) {

Loading…
Cancel
Save