zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

mc_pos_control: when close to current and previous, ajdust target velocity

sbg
Dennis Mannhart 8 years ago committed by Lorenz Meier
parent
c4c18caed4
commit
3e4ab5ed59
1 changed files with 118 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 181
      src/modules/mc_pos_control/mc_pos_control_main.cpp

181
src/modules/mc_pos_control/mc_pos_control_main.cpp
Unescape View File

@ -348,6 +348,8 @@ private: @@ -348,6 +348,8 @@ private:
bool in_auto_takeoff();
float get_vel_close(const matrix::Vector2f &unit_prev_to_current, const matrix::Vector2f &unit_current_to_next);
/**
* limit altitude based on several conditions
*/
@ -882,6 +884,49 @@ MulticopterPositionControl::in_auto_takeoff() @@ -882,6 +884,49 @@ MulticopterPositionControl::in_auto_takeoff()
_control_mode.flag_control_offboard_enabled;
}
float
MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_current,
const matrix::Vector2f &unit_current_to_next)
{
/* angle = cos(x) + 1.0
* angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */
float angle = 2.0f;
if (unit_current_to_next.length() > SIGMA_NORM) {
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_close = vel_cruise/5.0 (this means that at 90degrees
* the velocity at target should be 1/5 * cruising speed;
* angle = 2 -> vel_close = min_cruising_speed */
/* middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle = 1.0 = 90degrees */
float middle_cruise_speed = 2.0f * _min_cruise_speed.get();
/* sanity check: make sure middle cruise speed is always in between min and max*/
middle_cruise_speed = ((_min_cruise_speed.get() < middle_cruise_speed)
&& (get_cruising_speed_xy() > middle_cruise_speed)) ? middle_cruise_speed : _min_cruise_speed.get() + SIGMA_NORM;
/* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */
float a = -((middle_cruise_speed - get_cruising_speed_xy()) * (middle_cruise_speed - get_cruising_speed_xy())) /
(2.0f * middle_cruise_speed - get_cruising_speed_xy() - _min_cruise_speed.get());
float c = get_cruising_speed_xy() - a;
float b = (middle_cruise_speed - c) / a;
float vel_close = a * powf(b, angle) + c;
/* sanity check: vel_close needs to be in between max and min */
if ((vel_close - _min_cruise_speed.get()) < SIGMA_SINGLE_OP) {
vel_close = _min_cruise_speed.get();
} else if (vel_close > get_cruising_speed_xy()) {
vel_close = get_cruising_speed_xy();
}
return vel_close;
}
float
MulticopterPositionControl::get_cruising_speed_xy()
{
@ -1440,12 +1485,11 @@ void MulticopterPositionControl::control_auto(float dt) @@ -1440,12 +1485,11 @@ void MulticopterPositionControl::control_auto(float dt)
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
/* by default use current setpoint as is */
math::Vector<3> pos_sp = _curr_pos_sp;
/* line from previous to current and from pos to current */
matrix::Vector2f unit_prev_to_current((_curr_pos_sp(0) - _prev_pos_sp(0)), (_curr_pos_sp(1) - _prev_pos_sp(1)));
matrix::Vector2f vec_prev_to_current((_curr_pos_sp(0) - _prev_pos_sp(0)), (_curr_pos_sp(1) - _prev_pos_sp(1)));
matrix::Vector2f vec_pos_to_current((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1)));
@ -1456,10 +1500,19 @@ void MulticopterPositionControl::control_auto(float dt) @@ -1456,10 +1500,19 @@ void MulticopterPositionControl::control_auto(float dt)
&& ((pos_sp_diff.length()) < SIGMA_NORM);
/* only follow line if previous to current has a minimum distance */
if (unit_prev_to_current.length() > 0.1f && !stay_at_current_pos) {
if ((vec_prev_to_current.length() > _nav_rad.get()) && !stay_at_current_pos) {
/* normalize prev-current line (always > nav_rad) */
matrix::Vector2f unit_prev_to_current = vec_prev_to_current.normalized();
/* normalize prev-current line (always > 0.1) */
unit_prev_to_current = unit_prev_to_current.normalized();
/* unit vector from current to next */
matrix::Vector2f unit_current_to_next(0.0f, 0.0f);
if (next_setpoint_valid) {
unit_current_to_next = matrix::Vector2f((next_sp(0) - pos_sp(0)), (next_sp(1) - pos_sp(1)));
unit_current_to_next = (unit_current_to_next.length() > SIGMA_NORM) ? unit_current_to_next.normalized() :
unit_current_to_next;
}
/* point on line closest to pos */
matrix::Vector2f closest_point = matrix::Vector2f(_prev_pos_sp(0), _prev_pos_sp(1)) + unit_prev_to_current *
@ -1473,8 +1526,12 @@ void MulticopterPositionControl::control_auto(float dt) @@ -1473,8 +1526,12 @@ void MulticopterPositionControl::control_auto(float dt)
/* current velocity along track */
float vel_sp_along_track_prev = matrix::Vector2f(_vel_sp(0), _vel_sp(1)) * unit_prev_to_current;
bool close_to_current = vec_pos_to_current.length() < _target_threshold_xy.get();
bool close_to_prev = (vec_prev_to_pos.length() < _target_threshold_xy.get()) &&
(vec_prev_to_pos.length() < vec_pos_to_current.length());
/* 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();
bool is_2_target_threshold = vec_prev_to_current.length() >= 2.0f * _target_threshold_xy.get();
/* check if the current setpoint is behind */
bool current_behind = ((vec_pos_to_current * -1.0f) * unit_prev_to_current) > 0.0f;
@ -1504,83 +1561,81 @@ void MulticopterPositionControl::control_auto(float dt) @@ -1504,83 +1561,81 @@ void MulticopterPositionControl::control_auto(float dt)
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 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 */
min_cruise_speed = (min_cruise_speed > _min_cruise_speed.get()) ? min_cruise_speed : _min_cruise_speed.get();
} else if (close_to_prev) {
/* accelerate from previous setpoint towards current setpoint */
/* 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();
/* we are close to previous and current setpoint
* we first compute the start velocity when close to current septoint and use
* this velocity as final velocity when transition occurs from acceleration to deceleration.
* This ensures smooth transition */
float final_cruise_speed = get_cruising_speed_xy();
float target_threshold = _target_threshold_xy.get();
/* 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;
if (!is_2_target_threshold) {
/* 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;
}
/* set target threshold to half dist pre-current */
target_threshold = vec_prev_to_current.length() * 0.5f;
} else if (vec_pos_to_current.length() < _target_threshold_xy.get()) {
/* slow down when close to current setpoint */
/* velocity close to current setpoint with default zero if no next setpoint is available */
float vel_close = 0.0f;
float acceptance_radius = 0.0f;
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 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() > SIGMA_NORM) ? unit_current_to_next.normalized() :
unit_current_to_next;
/* we want to pass and need to compute the desired velocity close to current setpoint */
if (next_setpoint_valid && !(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
/* get velocity close to current that depends on angle between prev-current and current-next line */
vel_close = get_vel_close(unit_prev_to_current, unit_current_to_next);
acceptance_radius = _nav_rad.get();
}
/* angle = cos(x) + 1.0
* angle goes from 0 to 2 with 0 = large angle, 2 = small angle: 0 = PI ; 2 = PI*0 */
float angle = 2.0f;
/* compute velocity at transition where vehicle switches from acceleration to deceleration */
if ((target_threshold - acceptance_radius) < SIGMA_NORM) {
final_cruise_speed = vel_close;
if (unit_current_to_next.length() > SIGMA_NORM) {
angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f;
} else {
float slope = (get_cruising_speed_xy() - vel_close) / (_target_threshold_xy.get() - acceptance_radius);
final_cruise_speed = slope * (target_threshold - acceptance_radius) + vel_close;
final_cruise_speed = (final_cruise_speed > vel_close) ? final_cruise_speed : vel_close;
}
}
/* velocity close to target adjusted to angle
* vel_close = a *b ^x + c; where at angle = 0 -> vel_close = vel_cruise; angle = 1 -> vel_close = vel_cruise/5.0 (this means that at 90degrees
* the velocity at target should be 1/5 * cruising speed;
* angle = 2 -> vel_close = min_cruising_speed */
/* make sure final cruise speed is larger than minimum curise speed */
final_cruise_speed = (_min_cruise_speed.get() < final_cruise_speed) ? final_cruise_speed : _min_cruise_speed.get();
/* middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle = 1.0 = 90degrees */
float middle_cruise_speed = 1.0f;
/* compute the velocity along the track depending on distance close to previous setpoint */
if ((target_threshold - _nav_rad.get()) < SIGMA_NORM) {
vel_sp_along_track = _min_cruise_speed.get();
/* make sure min cruise speed is always smaller than middle cruise speed but larger than 0*/
float min_cruise_speed = (_min_cruise_speed.get() < middle_cruise_speed) ? _min_cruise_speed.get() : middle_cruise_speed
- 0.01f;
} else {
float slope = (final_cruise_speed - _min_cruise_speed.get()) / (target_threshold - _nav_rad.get());
vel_sp_along_track = slope * (vec_prev_to_pos.length() - _nav_rad.get()) + _min_cruise_speed.get();
}
/* make sure min cruise speed is larger than zero: this case should never occur unless _min_cruise_speed is negative */
min_cruise_speed = (min_cruise_speed < 0.0f) ? SIGMA_SINGLE_OP : min_cruise_speed;
/* sanity check: enforce minimum cruise speed */
vel_sp_along_track = (vel_sp_along_track < _min_cruise_speed.get()) ? _min_cruise_speed.get() : vel_sp_along_track;
/* 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;
}
/* from maximum cruise speed, minimum cruise speed and middle cruise speed compute constants a, b and c */
float a = -((middle_cruise_speed - get_cruising_speed_xy()) * (middle_cruise_speed - get_cruising_speed_xy())) /
(2.0f * middle_cruise_speed - get_cruising_speed_xy() - min_cruise_speed);
float c = get_cruising_speed_xy() - a;
float b = (middle_cruise_speed - c) / a;
float vel_close = a * powf(b, angle) + c;
} else if (close_to_current) {
/* slow down when close to current setpoint */
/* sanity check: vel_close needs to be in between max and min */
vel_close = (PX4_ISFINITE(vel_close)) ? vel_close : _min_cruise_speed.get();
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 */
if (!(vel_close >= min_cruise_speed) && !(vel_close <= get_cruising_speed_xy())) {
vel_close = min_cruise_speed;
}
/* get velocity close to current that depends on angle between prev-current and current-next line */
float vel_close = get_vel_close(unit_prev_to_current, unit_current_to_next);
/* compute velocity along line which depends on distance to current setpoint */
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;
if (vec_closest_to_current.length() < _nav_rad.get()) {
vel_sp_along_track = _min_cruise_speed.get();
/* make sure vel_sp_along_track is positive */
vel_sp_along_track = (vel_sp_along_track < 0.0f) ? SIGMA_SINGLE_OP : vel_sp_along_track;
} else {
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() - _nav_rad.get()) + 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)) {

Write Preview
Loading…
Cancel
Save