yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Dennis Mannhart
2018-01-10 09:26:33 +01:00
committed by Beat Küng
parent 13eb79ffb8
commit 59f3de192f
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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) {