Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

AutoLineSmoothVel - Replace min/max absolute constraints with a function named constrainAbs

Browse Source 
This function constrain the absolute value of some value but keeps the original sign.
This commit is contained in:
bresch
2019-09-13 14:56:09 +02:00
committed by Mathieu Bresciani
parent 3c5b24037a
commit 47401d1177
2 changed files with 5 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
+4 -12
View File
@@ -147,15 +147,9 @@ inline float FlightTaskAutoLineSmoothVel::_constrainOneSide(float val, float con
return math::constrain(val, min, max);
}
float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float constraint)
float FlightTaskAutoLineSmoothVel::_constrainAbs(float val, float min, float max)
{
float constraint_abs = fabsf(constraint);
return math::constrain(val, -constraint_abs, constraint_abs);
}
float FlightTaskAutoLineSmoothVel::_maxAbs(float val, float constraint)
{
return math::sign(val) * math::max(fabsf(val), fabsf(constraint));
return math::sign(val) * math::constrain(fabsf(val), fabsf(min), fabsf(max));
}
void FlightTaskAutoLineSmoothVel::_initEkfResetCounters()
@@ -280,8 +274,6 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
// to force the drone to pass the waypoint with a desired speed
Vector2f u_prev_to_target_xy((_target - _prev_wp).unit_or_zero());
vel_min_xy = u_prev_to_target_xy * _getSpeedAtTarget();
vel_min_xy(0) = fabsf(vel_min_xy(0));
vel_min_xy(1) = fabsf(vel_min_xy(1));
} else {
// The drone has to change altitude, stop at the waypoint
@@ -290,8 +282,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) = _maxAbs(_constrainAbs(vel_sp_xy(0), vel_max_xy(0)), vel_min_xy(0));
vel_sp_constrained_xy(1) = _maxAbs(_constrainAbs(vel_sp_xy(1), vel_max_xy(1)), vel_min_xy(1));
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));
for (int i = 0; i < 2; i++) {
// If available, constrain the velocity using _velocity_setpoint(.)
src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
+1 -2
View File
@@ -73,8 +73,7 @@ protected:
bool _checkTakeoff() override { return _want_takeoff; };
inline float _constrainOneSide(float val, float constraint); /**< Constrain val between INF and constraint */
inline float _constrainAbs(float val, float constraint); /**< Constrain the absolute value of val */
inline float _maxAbs(float val, float constraint); /**< Maximum of val and constraint with the sign of val */
inline float _constrainAbs(float val, float min, float max); /**< Constrain the absolute value of val to be at least min and at most max */
void _initEkfResetCounters();
void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */