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

ManualSmoothingXY: brake with large default jerk max

Browse Source
This commit is contained in:
Dennis Mannhart 2018-02-19 10:52:23 +01:00 committed by Beat Küng
parent 25c33d18b0
commit 59bddf339a
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -166,8 +166,14 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
switch (intention) {
case Intention::brake: {
/* First iteration where user demands brake */
if (intention != _intention) {
if (_intention == Intention::direction_change) {
/* Vehicle switched from direciton change to brake.
* Don't do any slwerate but rather just stop.
*/
_jerk_state_dependent = 1e6f;
_vel_sp_prev = vel;
} else if (intention != _intention) {
/* we start braking with lowest acceleration
* This make stopping smoother. */
_acc_state_dependent = _dec_xy_min;
@ -176,12 +182,12 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
* that the vehicle will stop much quicker at large speed but
* very slow at low speed.
*/
_jerk_state_dependent = _jerk_max; // default
_jerk_state_dependent = 1e6f; // default
if (_jerk_max > _jerk_min) {
_jerk_state_dependent = (_jerk_max - _jerk_min)
/ _vel_manual * vel.length() + _jerk_min;
_jerk_state_dependent = math::min((_jerk_max - _jerk_min)
/ _vel_manual * vel.length() + _jerk_min, _jerk_max);
}
/* Since user wants to brake smoothly but NOT continuing to fly