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

motion_planning: In VelovitySmoothing, mark const functions const

Browse Source
This commit is contained in:
Thomas Debrunner
2021-10-13 21:25:43 -04:00
committed by Daniel Agar
parent fed234de81
commit f08f2a340d
2 changed files with 18 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/motion_planning/VelocitySmoothing.cpp
+9 -9
View File
@@ -55,7 +55,7 @@ void VelocitySmoothing::reset(float accel, float vel, float pos)
_state_init = _state;
}
float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max)
float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, float a_max) const
{
/* Check maximum acceleration, saturate and recompute T1 if needed */
float accel_T1 = a0 + j_max * T1;
@@ -71,7 +71,7 @@ float VelocitySmoothing::saturateT1ForAccel(float a0, float j_max, float T1, flo
return T1_new;
}
float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max) const
{
float delta = 2.f * a0 * a0 + 4.f * j_max * v3;
@@ -101,7 +101,7 @@ float VelocitySmoothing::computeT1(float a0, float v3, float j_max, float a_max)
return math::max(T1, 0.f);
}
float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max)
float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max, float a_max) const
{
float a = -j_max;
float b = j_max * T123 - a0;
@@ -138,7 +138,7 @@ float VelocitySmoothing::computeT1(float T123, float a0, float v3, float j_max,
}
float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max)
float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float j_max) const
{
float T2 = 0.f;
@@ -151,13 +151,13 @@ float VelocitySmoothing::computeT2(float T1, float T3, float a0, float v3, float
return math::max(T2, 0.f);
}
float VelocitySmoothing::computeT2(float T123, float T1, float T3)
float VelocitySmoothing::computeT2(float T123, float T1, float T3) const
{
float T2 = T123 - T1 - T3;
return math::max(T2, 0.f);
}
float VelocitySmoothing::computeT3(float T1, float a0, float j_max)
float VelocitySmoothing::computeT3(float T1, float a0, float j_max) const
{
float T3 = a0 / j_max + T1;
return math::max(T3, 0.f);
@@ -179,7 +179,7 @@ void VelocitySmoothing::updateDurations(float vel_setpoint)
}
}
int VelocitySmoothing::computeDirection()
int VelocitySmoothing::computeDirection() const
{
// Compute the velocity at which the trajectory will be
// when the acceleration will be zero
@@ -197,7 +197,7 @@ int VelocitySmoothing::computeDirection()
return direction;
}
float VelocitySmoothing::computeVelAtZeroAcc()
float VelocitySmoothing::computeVelAtZeroAcc() const
{
float vel_zero_acc = _state.v;
@@ -225,7 +225,7 @@ void VelocitySmoothing::updateDurationsMinimizeTotalTime()
_T2 = computeT2(_T1, _T3, _state.a, delta_v, jerk_max_T1);
}
Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d)
Trajectory VelocitySmoothing::evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const
{
Trajectory traj;
float jt = d * j;
src/lib/motion_planning/VelocitySmoothing.hpp
+9 -9
View File
@@ -141,43 +141,43 @@ private:
* Compute the direction of the jerk to be applied in order to drive the current state
* to the desired one
*/
int computeDirection();
int computeDirection() const;
/**
* Compute the velocity at which the trajectory will be if the maximum jerk is applied
* during the time required to cancel the current acceleration
*/
float computeVelAtZeroAcc();
float computeVelAtZeroAcc() const;
/**
* Compute increasing acceleration time
*/
inline float computeT1(float a0, float v3, float j_max, float a_max);
inline float computeT1(float a0, float v3, float j_max, float a_max) const;
/**
* Compute increasing acceleration time using total time constraint
*/
inline float computeT1(float T123, float a0, float v3, float j_max, float a_max);
inline float computeT1(float T123, float a0, float v3, float j_max, float a_max) const;
/**
* Saturate T1 in order to respect the maximum acceleration constraint
*/
inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max);
inline float saturateT1ForAccel(float a0, float j_max, float T1, float a_max) const;
/**
* Compute constant acceleration time
*/
inline float computeT2(float T1, float T3, float a0, float v3, float j_max);
inline float computeT2(float T1, float T3, float a0, float v3, float j_max) const;
/**
* Compute constant acceleration time using total time constraint
*/
inline float computeT2(float T123, float T1, float T3);
inline float computeT2(float T123, float T1, float T3) const;
/**
* Compute decreasing acceleration time
*/
inline float computeT3(float T1, float a0, float j_max);
inline float computeT3(float T1, float a0, float j_max) const;
/**
* Compute the jerk, acceleration, velocity and position
@@ -189,7 +189,7 @@ private:
* @param t current time
* @param d direction
*/
inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d);
inline Trajectory evaluatePoly(float j, float a0, float v0, float x0, float t, int d) const;
/* Input */
float _vel_sp{0.0f};