dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AutoLineSmoothVel: fix constrain priority for autocontinue. 

The constrainAbs function was not prioritizing the minimum value
that produces the autocontinue behaviour. This caused zig-zag
paths when the waypoints were almost -but not exactly- aligned.
This commit is contained in:
bresch 2019-10-25 11:08:04 +02:00 committed by Daniel Agar
parent 0d895f2a0a
commit 6535d5123e
2 changed files with 21 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
View File

@ -168,7 +168,7 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj()
* Example: - if the constrain is -5, the value will be constrained between -5 and 0
* - if the constrain is 5, the value will be constrained between 0 and 5
*/
inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float constraint)
{
const float min = (constraint < FLT_EPSILON) ? constraint : 0.f;
const float max = (constraint > FLT_EPSILON) ? constraint : 0.f;
@ -176,12 +176,12 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
return math::constrain(val, min, max);
}
float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max)
float FlightTaskAutoLineSmoothVel::_constrainAbsPrioritizeMin(float val, float min, float max)
{
return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max));
return math::sign(val) * math::max(math::min(fabsf(val), fabsf(max)), fabsf(min));
}
float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget() const
{
// Compute the maximum allowed speed at the waypoint assuming that we want to
// connect the two lines (prev-current and current-next)
@ -217,7 +217,7 @@ float FlightTaskAutoLineSmoothVel::_getSpeedAtTarget()
return speed_at_target;
}
float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance)
float FlightTaskAutoLineSmoothVel::_getMaxSpeedFromDistance(float braking_distance) const
{
float max_speed = math::trajectory::computeMaxSpeedFromBrakingDistance(_param_mpc_jerk_auto.get(),
_param_mpc_acc_hor.get(),
@ -277,8 +277,8 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
// Constrain the norm of each component using min and max values
Vector2f vel_sp_constrained_xy;
vel_sp_constrained_xy(0) = _constrainAbs(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0));
vel_sp_constrained_xy(1) = _constrainAbs(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1));
vel_sp_constrained_xy(0) = _constrainAbsPrioritizeMin(vel_sp_xy(0), vel_min_xy(0), vel_max_xy(0));
vel_sp_constrained_xy(1) = _constrainAbsPrioritizeMin(vel_sp_xy(1), vel_min_xy(1), vel_max_xy(1));
for (int i = 0; i < 2; i++) {
// If available, constrain the velocity using _velocity_setpoint(.)

18
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
View File

@ -77,11 +77,21 @@ protected:
void _generateHeading();
bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
inline float _constrainAbs(float val, float min, float max); /**< Constrain absolute value of val between min and max */
static float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
float _getSpeedAtTarget();
float _getMaxSpeedFromDistance(float braking_distance);
/**
* Constrain the abs value below max but above min
* Min can be larger than max and has priority over it
* The whole computation is done on the absolute values but the returned
* value has the sign of val
* @param val the value to constrain and boost
* @param min the minimum value that the function should return
* @param max the value by which val is constrained before the boost is applied
*/
static float _constrainAbsPrioritizeMin(float val, float min, float max);
float _getSpeedAtTarget() const;
float _getMaxSpeedFromDistance(float braking_distance) const;
void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
void _updateTrajConstraints();