From e51e52f425ec6ea4a4318c655f585abccbd46f62 Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Thu, 4 May 2017 19:06:07 +0200 Subject: [PATCH] mc_pos_control: reorder auto logic and ensure that nan gets caught --- .../mc_pos_control/mc_pos_control_main.cpp | 120 ++++++++++++------ 1 file changed, 83 insertions(+), 37 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 016123d51e..0b5d039cc9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1420,17 +1420,12 @@ void MulticopterPositionControl::control_auto(float dt) } } + + /* create new _pos_sp from triplets */ if (current_setpoint_valid && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) { - float cruising_speed_xy = get_cruising_speed_xy(); - float cruising_speed_z = (_curr_pos_sp(2) > _pos(2)) ? _params.vel_max_down : _params.vel_max_up; - - /* scaled space: 1 == position error resulting max allowed speed */ - math::Vector<3> cruising_speed(cruising_speed_xy, cruising_speed_xy, cruising_speed_z); - - - /* we follow line depending on setpoint type */ + /* only follow previous-current-line for specific triplet type */ if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { @@ -1438,47 +1433,74 @@ void MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp = _curr_pos_sp; + + /* line from previous to current */ matrix::Vector2f unit_prev_to_current((_curr_pos_sp(0) - _prev_pos_sp(0)), (_curr_pos_sp(1) - _prev_pos_sp(1))); - /* follow line */ + /* only follow line if previous to current has a minimum distance */ if (unit_prev_to_current.length() > 0.1f) { - /* unit vector from previous to current */ + /* normalize prev-current line (always > 0.1) */ unit_prev_to_current = unit_prev_to_current.normalized(); /* orthogonal distance from current position to unit_prev_to_current */ matrix::Vector2f closest_point = matrix::Vector2f(_prev_pos_sp(0), _prev_pos_sp(1)) + unit_prev_to_current * (matrix::Vector2f((_pos(0) - _prev_pos_sp(0)), (_pos(1) - _prev_pos_sp(1))) * unit_prev_to_current); - /* check if we need to adjust position setpoint based on cruise velocity */ + /* compute vector from position-current and previous-position */ matrix::Vector2f vec_pos_to_current((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1))); matrix::Vector2f vec_prev_to_pos((_pos(0) - _prev_pos_sp(0)), (_pos(1) - _prev_pos_sp(1))); + /* indicates if we are at least half the distance from previos to current close to previous */ + bool close_to_prev = ((_curr_pos_sp - _prev_pos_sp).length() * 0.5f) > vec_prev_to_pos.length(); + + /* check if the current setpoint is behind */ + bool current_behind = ((vec_pos_to_current * -1.0f) * unit_prev_to_current) > 0.0f; + + /* check if the previous is in front */ + bool previous_in_front = (vec_prev_to_pos * unit_prev_to_current) < 0.0f; + + /* default velocity along line prev-current */ float vel_sp_along_track = get_cruising_speed_xy(); - /* accelerate from previous setpoint */ - if ((vec_prev_to_pos.length() < _target_threshold_xy.get()) - && (((_curr_pos_sp - _prev_pos_sp).length() * 0.5f) > vec_prev_to_pos.length())) { - /* accelerate */ + /* compute velocity setpoint along track */ + if (previous_in_front || current_behind) { + + /* just use the default velocity along track */ + + } else if ((vec_prev_to_pos.length() < _target_threshold_xy.get()) && close_to_prev) { + /* accelerate from previous setpoint towards current setpoint */ + + /* the minimum cruise speed is the current velocity: this ensures that we do not slow down when moving towards current setpoint */ float min_cruise_speed = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); + + /* we don't want to get stucked with zero velociyt setpoint: enforce a min cruise speed */ min_cruise_speed = (min_cruise_speed > _min_cruise_speed.get()) ? min_cruise_speed : _min_cruise_speed.get(); + + /* make sure min cruise speed is smaller as maximum cruise speed */ + min_cruise_speed = (min_cruise_speed < get_cruising_speed_xy()) ? min_cruise_speed : get_cruising_speed_xy(); + + /* compute the velocity along the track depending on distance close to previous setpoint*/ float slope = (get_cruising_speed_xy() - min_cruise_speed) / _target_threshold_xy.get(); vel_sp_along_track = slope * vec_prev_to_pos.length() + min_cruise_speed; - } - /* slow down */ - else if (vec_pos_to_current.length() < _target_threshold_xy.get()) { + } else if (vec_pos_to_current.length() < _target_threshold_xy.get()) { + /* slow down when close to current setpoint */ - /* angle prev-current-next defines speed close to waypoint */ - if (next_setpoint_valid) { + if (next_setpoint_valid && !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + /* since we have a next setpoint use the angle prev-current-next to compute velocity setpoint limit */ - /* unit vector to from current to next */ + /* unit vector from current to next */ matrix::Vector2f unit_current_to_next((next_sp(0) - pos_sp(0)), (next_sp(1) - pos_sp(1))); - unit_current_to_next = (unit_current_to_next.length() > 0.0f) ? unit_current_to_next.normalized() : + unit_current_to_next = (unit_current_to_next.length() > 0.01f) ? unit_current_to_next.normalized() : unit_current_to_next; - /* angle goes from 0 to 2 with 0 being large angle, 2 being tight angle: 0 = PI ; 2 = PI*0 */ - float angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f; + /* angle goes from 0 to 2 with 0 being large angle, 2 being small angle: 0 = PI ; 2 = PI*0 */ + float angle = 2.0f; + + if (unit_current_to_next.length() > 0.01f) { + angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f; + } /* velocity close to target adjusted to angle * vel_close = a *b ^x + c; where at angle = 0 -> vel_close = vel_cruise; angle = 1 -> vel_cruise/4.0 (this means that at 90degrees @@ -1491,6 +1513,11 @@ void MulticopterPositionControl::control_auto(float dt) float b = (M - c) / a; float vel_close = a * powf(b, angle) + c; + /* insanity check */ + vel_close = (vel_close < get_cruising_speed_xy()) ? vel_close : get_cruising_speed_xy(); + vel_close = fabsf(vel_close); + + /* compute velocity along line dependent on distance to current setpoint */ float slope = (get_cruising_speed_xy() - vel_close) / _target_threshold_xy.get(); vel_sp_along_track = slope * vec_pos_to_current.length() + vel_close; @@ -1501,18 +1528,21 @@ void MulticopterPositionControl::control_auto(float dt) } } - /* we want position setpoint not farther away then cruise speed */ - /* we adjust position setpoint */ + + /*compute velocity orthogonal to prev-current-line to position*/ matrix::Vector2f vec_pos_to_closest = closest_point - matrix::Vector2f(_pos(0), _pos(1)); float vel_sp_orthogonal = vec_pos_to_closest.length() * _params.pos_p(0); + + /* compute the cruise speed from velocity along line and orthogonal velocity setpoint */ float cruise_sp_mag = sqrtf(vel_sp_orthogonal * vel_sp_orthogonal + vel_sp_along_track * vel_sp_along_track); - /* check on which section of the track the vehicle is*/ - bool current_behind = ((vec_pos_to_current * -1.0f) * unit_prev_to_current) > 0.0f; + /* insanity check */ + cruise_sp_mag = (PX4_ISFINITE(cruise_sp_mag)) ? cruise_sp_mag : vel_sp_orthogonal; - /* orthogonal velcoity is smaller then cruise speed */ + /* orthogonal velocity setpoint is smaller then cruise speed */ if (vel_sp_orthogonal < get_cruising_speed_xy() && !current_behind) { - /* we need to limit vel_sp_along_track such that cruise speed is never exceeded */ + + /* we need to limit vel_sp_along_track such that cruise speed is never exceeded but still can keep velocity orthogonal to track */ if (cruise_sp_mag > get_cruising_speed_xy()) { vel_sp_along_track = sqrtf(get_cruising_speed_xy() * get_cruising_speed_xy() - vel_sp_orthogonal * vel_sp_orthogonal); } @@ -1521,29 +1551,36 @@ void MulticopterPositionControl::control_auto(float dt) pos_sp(1) = closest_point(1) + unit_prev_to_current(1) * vel_sp_along_track / _params.pos_p(0); } else { - /* we are more than cruise_speed away from track */ - /* check on which section we are with default as closest_point */ - bool previous_in_front = (vec_prev_to_pos * unit_prev_to_current) < 0.0f; + /* we are more than cruise_speed away from track or current is behind */ + /* if previous is in front just directly to previous point */ if (previous_in_front) { vec_pos_to_closest(0) = _prev_pos_sp(0) - _pos(0); vec_pos_to_closest(1) = _prev_pos_sp(1) - _pos(1); } + /* if current setpoint is behind just go directly to current setpoint */ if (current_behind) { - /* we already passed current_sp */ vec_pos_to_closest(0) = _curr_pos_sp(0) - _pos(0); vec_pos_to_closest(1) = _curr_pos_sp(1) - _pos(1); } - float cruise_sp = vec_pos_to_current.length() * _params.pos_p(0); + /* make sure that we never exceed maximum cruise speed */ + float cruise_sp = vec_pos_to_closest.length() * _params.pos_p(0); if (cruise_sp > get_cruising_speed_xy()) { cruise_sp = get_cruising_speed_xy(); } - pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruise_sp / _params.pos_p(0); - pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruise_sp / _params.pos_p(1); + /* if we close to closest point then just go to closest point */ + if (vec_pos_to_closest.length() > 0.1f) { + pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruise_sp / _params.pos_p(0); + pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruise_sp / _params.pos_p(1); + + } else { + pos_sp(0) = closest_point(0); + pos_sp(1) = closest_point(0); + } } } @@ -1557,6 +1594,14 @@ void MulticopterPositionControl::control_auto(float dt) _vel_max_xy = get_cruising_speed_xy(); } + /* insanity check */ + if (!(PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1)) && + PX4_ISFINITE(_pos_sp(2)))) { + + PX4_WARN("Auto: Position setpoint not finite"); + _pos_sp = _pos; + } + /* update yaw setpoint if needed */ if (_pos_sp_triplet.current.yawspeed_valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) { @@ -1609,6 +1654,7 @@ void MulticopterPositionControl::control_auto(float dt) _vel_sp.zero(); _run_pos_control = false; _run_alt_control = false; + } }