fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 18:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Trajectory - Overload integrate function to allow for custom integration period

Browse Source
This commit is contained in:
bresch
2018-11-06 22:17:00 +01:00
committed by Roman Bapst
parent 7073187a48
commit aa586ca327
2 changed files with 15 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
+11 -5
View File
@@ -139,10 +139,10 @@ float VelocitySmoothing::computeT3(float T1, float accel_prev, float max_jerk)
return math::max(T3, 0.f);
}
void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
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;
accel_out = jerk * dt + accel_prev;
if (accel_out > _max_accel) {
accel_out = _max_accel;
@@ -151,7 +151,7 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
accel_out = -_max_accel;
}
vel_out = _dt * 0.5f * (accel_out + accel_prev) + vel_prev;
vel_out = dt * 0.5f * (accel_out + accel_prev) + vel_prev;
if (vel_out > _max_vel) {
vel_out = _max_vel;
@@ -160,7 +160,7 @@ void VelocitySmoothing::integrateT(float jerk, float accel_prev, float vel_prev,
vel_out = -_max_vel;
}
pos_out = _dt / 3.f * (vel_out + accel_prev * _dt * 0.5f + 2.f * vel_prev) + _pos;
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)
@@ -216,10 +216,16 @@ void VelocitySmoothing::updateDurations(float T123)
void VelocitySmoothing::integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth,
float &pos_setpoint_smooth)
{
integrate(_dt, accel_setpoint_smooth, vel_setpoint_smooth, pos_setpoint_smooth);
}
void VelocitySmoothing::integrate(float dt,float &accel_setpoint_smooth, float &vel_setpoint_smooth,
float &pos_setpoint_smooth)
{
/* Integrate the trajectory */
float accel_new, vel_new, pos_new;
integrateT(_jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
integrateT(dt, _jerk, _accel, _vel, _pos, accel_new, vel_new, pos_new);
/* Apply correct jerk (min, max or zero) */
if (_T1 > 0.f) {