fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 17:14:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

VelocitySmoothing: refactor local scope t variables instead of array

Browse Source
This commit is contained in:
Matthias Grob 2019-09-17 09:07:24 +02:00
parent da8ac8cafb
commit 270e12a4df
1 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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) {