Browse Source

ManualSmoothingXY: velocity as criteria for direction change to prenvent fast acceleration at low spped

sbg
Dennis Mannhart 7 years ago committed by Beat Küng
parent
commit
59f3de192f
  1. 5
      src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp

5
src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp

@ -140,10 +140,11 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V @@ -140,10 +140,11 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp, const matrix::V
const bool is_aligned = (vel_sp_heading * vel_heading) > -0.707f;
/* In almost all cases we want to use acceleration.
* Only use direction change if not aligned, no yawspeed demand and demand larger than 0.7 of max speed.
* Only use direction change if not aligned, no yawspeed demand, demand larger than 0.7 of max speed and velocity larger than 2m/s.
* Only use deceleration if stick input is lower than previous setpoint, aligned and no yawspeed demand. */
bool yawspeed_demand = fabsf(yawrate_sp) > 0.05f && PX4_ISFINITE(yawrate_sp);
bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand;
bool direction_change = !is_aligned && (vel_sp.length() > 0.7f * _vel_manual) && !yawspeed_demand
&& (vel.length() > 2.0f);
bool deceleration = is_aligned && (vel_sp.length() < _vel_sp_prev.length()) && !yawspeed_demand;
if (direction_change) {

Loading…
Cancel
Save