From 7734279f61b7441a8c9bce0c969e66fb3193d7cf Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Fri, 5 May 2017 18:44:31 +0200 Subject: [PATCH] mc_pos_control: smooth transition between waypoint updates --- .../mc_pos_control/mc_pos_control_main.cpp | 49 +++++++++++++++---- 1 file changed, 39 insertions(+), 10 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 4eefd5f211..388e142296 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -147,7 +147,7 @@ private: control::BlockParamFloat _min_cruise_speed; /**< minimum cruising speed when passing waypoint */ control::BlockParamFloat _velocity_hor_manual; /**< target velocity in manual controlled mode at full speed*/ control::BlockParamFloat _takeoff_ramp_time; /**< time contant for smooth takeoff ramp */ - + control::BlockParamFloat _nav_rad; /**< radius that is used by navigator that defines when to update triplets */ control::BlockDerivative _vel_x_deriv; control::BlockDerivative _vel_y_deriv; control::BlockDerivative _vel_z_deriv; @@ -405,6 +405,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _velocity_hor_manual(this, "VEL_MAN_MAX", true), _takeoff_ramp_time(this, "TKO_RAMP_T", true), _min_cruise_speed(this, "CRUISE_MIN", true), + _nav_rad(this, "NAV_ACC_RAD", false), _vel_x_deriv(this, "VELD"), _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), @@ -1453,13 +1454,18 @@ void MulticopterPositionControl::control_auto(float dt) /* 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 */ + /* point on line closest to pos */ 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); + matrix::Vector2f vec_closest_to_current((_curr_pos_sp(0) - closest_point(0)), (_curr_pos_sp(1) - closest_point(1))); /* compute vector from position-current and previous-position */ matrix::Vector2f vec_prev_to_pos((_pos(0) - _prev_pos_sp(0)), (_pos(1) - _prev_pos_sp(1))); + /* current velocity along track */ + //float vel_along_track = matrix::Vector2f(_vel(0), _vel(1)) * unit_prev_to_current; + float vel_sp_along_track_prev = matrix::Vector2f(_vel_sp(0), _vel_sp(1)) * unit_prev_to_current; + /* indicates if we are at least half the distance from previous to current close to previous */ bool close_to_prev = ((_curr_pos_sp - _prev_pos_sp).length() * 0.5f) > vec_prev_to_pos.length(); @@ -1472,8 +1478,12 @@ void MulticopterPositionControl::control_auto(float dt) /* default velocity along line prev-current */ float vel_sp_along_track = get_cruising_speed_xy(); - /* compute velocity setpoint along track */ - if (previous_in_front) { + /* + * compute velocity setpoint along track + */ + + /* only go directly to previous setpoint if more than 5m away and previous in front*/ + if (previous_in_front && (vec_prev_to_pos.length() > 5.0f)) { /* just use the default velocity along track */ vel_sp_along_track = vec_prev_to_pos.length() * _params.pos_p(0); @@ -1483,14 +1493,14 @@ void MulticopterPositionControl::control_auto(float dt) } } else if (current_behind) { - + /* go directly to current setpoint */ vel_sp_along_track = vec_pos_to_current.length() * _params.pos_p(0); vel_sp_along_track = (vel_sp_along_track < get_cruising_speed_xy()) ? vel_sp_along_track : get_cruising_speed_xy(); } 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 */ + /* the minimum cruise speed is the previous velocity setpoint along track: 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 velocity setpoint: enforce a min cruise speed */ @@ -1499,10 +1509,16 @@ void MulticopterPositionControl::control_auto(float dt) /* 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*/ + /* compute the velocity along the track depending on distance close to previous setpoint with assumption that + * at previous setpoint the vehicle had zero velocity */ 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; + /* take over the previous velocity setpoint along track if larger since we want to accelerate */ + if (vel_sp_along_track_prev > vel_sp_along_track) { + vel_sp_along_track = vel_sp_along_track_prev; + } + } else if (vec_pos_to_current.length() < _target_threshold_xy.get()) { /* slow down when close to current setpoint */ @@ -1553,14 +1569,27 @@ void MulticopterPositionControl::control_auto(float dt) } /* compute velocity along line which depends 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; + float slope = (get_cruising_speed_xy() - vel_close) / (_target_threshold_xy.get() - _nav_rad.get()) ; + vel_sp_along_track = slope * (vec_closest_to_current.length()) + vel_close; + + /* since we want to slow down take over previous velocity setpoint along track if it was lower */ + if ((vel_sp_along_track_prev < vel_sp_along_track) && (vel_sp_along_track * vel_sp_along_track_prev > 0.0f)) { + vel_sp_along_track = vel_sp_along_track_prev; + } + + /* make sure that vel_sp_along track is at least min */ + vel_sp_along_track = (vel_sp_along_track < vel_close) ? vel_close : vel_sp_along_track; } else { /* we want to stop at current setpoint */ float slope = (get_cruising_speed_xy()) / _target_threshold_xy.get(); - vel_sp_along_track = slope * (vec_pos_to_current.length()); + vel_sp_along_track = slope * (vec_closest_to_current.length()); + + /* since we want to slow down take over previous velocity setpoint along track if it was lower */ + if (vel_sp_along_track_prev < vel_sp_along_track) { + vel_sp_along_track = vel_sp_along_track_prev; + } } }