yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00: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-10-16 09:58:32 +02: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

16
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.cpp
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) {

5
src/lib/FlightTasks/tasks/Utility/VelocitySmoothing.hpp
View File

@@ -77,11 +77,14 @@ public:
/**
* Generate the trajectory (acceleration, velocity and position) by integrating the current jerk
* @param dt optional integration period. If not given, the integration period provided during updateDuration call is used.
* A dt different from the one given during the computation of T1-T3 can be used to fast-forward or slow-down the trajectory.
* @param acc_setpoint_smooth returned smoothed acceleration setpoint
* @param vel_setpoint_smooth returned smoothed velocity setpoint
* @param pos_setpoint_smooth returned smoothed position setpoint
*/
void integrate(float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
void integrate(float dt, float &accel_setpoint_smooth, float &vel_setpoint_smooth, float &pos_setpoint_smooth);
/* Get / Set constraints (constraints can be updated at any time) */
float getMaxJerk() const { return _max_jerk; }
@@ -142,7 +145,7 @@ private:
/**
* Integrate the jerk, acceleration and velocity to get the new setpoints and states.
*/
inline void integrateT(float jerk, float accel_prev, float vel_prev, float pos_prev,
inline void integrateT(float dt, float jerk, float accel_prev, float vel_prev, float pos_prev,
float &accel_out, float &vel_out, float &pos_out);
/* Inputs */