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

mc_pos_control auto: replace min_cruise_speed with cruise_speed_90; take care of case when cruise_speed_90 is exactly in the middle of max and min

Browse Source
This commit is contained in:
Dennis Mannhart 2017-06-13 18:10:13 +02:00 committed by Lorenz Meier
parent 3538f028b4
commit 947d63fb11
2 changed files with 69 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -410,8 +410,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
_hold_dz(this, "HOLD_DZ"),
_acceleration_hor_max(this, "ACC_HOR_MAX", true),
_deceleration_hor_max(this, "DEC_HOR_MAX", true),
_min_cruise_speed(this, "CRUISE_MIN", true),
_velocity_hor_manual(this, "VEL_MAN_MAX", true),
_cruise_speed_90(this, "CRUISE_90", true),
_takeoff_ramp_time(this, "TKO_RAMP_T", true),
_min_cruise_speed(this, "CRUISE_MIN", true),
_nav_rad(this, "NAV_ACC_RAD", false),
@ -890,18 +890,33 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
const matrix::Vector2f &unit_current_to_next)
{
/* minimum cruise speed when passing waypoint */
float min_cruise_speed = 1.0f;
/* make sure that cruise speed is larger than minimum*/
if ((get_cruising_speed_xy() - _min_cruise_speed.get()) < SIGMA_NORM) {
if ((get_cruising_speed_xy() - min_cruise_speed) < SIGMA_NORM) {
return get_cruising_speed_xy();
}
/* middle cruise speed is a number between maximum cruising speed and minimum cruising speed and corresponds to speed at angle of 90degrees
* it needs to be alwawys larger than minimum cruise speed */
float middle_cruise_speed = 3.0f * (_min_cruise_speed.get() + SIGMA_NORM);
* it needs to be always larger than minimum cruise speed */
float middle_cruise_speed = _cruise_speed_90.get();
if ((middle_cruise_speed - min_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = min_cruise_speed + SIGMA_NORM;
}
/* make sure that cruise speed is larger than middle cruise speed, otherwise set it to half of max/min*/
if ((get_cruising_speed_xy() - middle_cruise_speed) < SIGMA_NORM) {
middle_cruise_speed = (get_cruising_speed_xy() + _min_cruise_speed.get()) * 0.5f;
middle_cruise_speed = (get_cruising_speed_xy() + min_cruise_speed) * 0.5f;
}
/* if middle cruise speed is exactly in the middle, then compute
* vel_close linearly
*/
bool use_linear_approach = false;
if (((get_cruising_speed_xy() + min_cruise_speed) * 0.5f) - middle_cruise_speed < SIGMA_NORM) {
use_linear_approach = true;
}
/* angle = cos(x) + 1.0
@ -912,21 +927,35 @@ MulticopterPositionControl::get_vel_close(const matrix::Vector2f &unit_prev_to_c
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 = middle_cruise_speed (this means that at 90degrees
* the velocity at target is middle_cruise_speed);
* angle = 2 -> vel_close = min_cruising_speed */
/* compute velocity target close to waypoint */
float vel_close;
/* 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;
if (use_linear_approach) {
/* velocity close to target adjusted to angle
* vel_close = m*x+q
*/
float slope = -(get_cruising_speed_xy() - min_cruise_speed) / 2.0f;
vel_close = slope * angle + get_cruising_speed_xy();
} else {
/* 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 = middle_cruise_speed (this means that at 90degrees
* the velocity at target is middle_cruise_speed);
* angle = 2 -> vel_close = min_cruising_speed */
/* 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;
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();
if ((vel_close - min_cruise_speed) < SIGMA_SINGLE_OP) {
vel_close = min_cruise_speed;
} else if (vel_close > get_cruising_speed_xy()) {
vel_close = get_cruising_speed_xy();
@ -1676,6 +1705,10 @@ void MulticopterPositionControl::control_auto(float dt)
target_threshold_xy = _nav_rad.get();
}
if ((target_threshold - _nav_rad.get()) < SIGMA_NORM) {
target_threshold = _nav_rad.get();
}
/* velocity close to current setpoint with default zero if no next setpoint is available */
float vel_close = 0.0f;
float acceptance_radius = 0.0f;
@ -1696,28 +1729,20 @@ void MulticopterPositionControl::control_auto(float dt)
final_cruise_speed = slope * (target_threshold_xy - acceptance_radius) + vel_close;
final_cruise_speed = (final_cruise_speed > vel_close) ? final_cruise_speed : vel_close;
}
}
/* 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();
/* make sure final cruise speed is larger than 0*/
final_cruise_speed = (final_cruise_speed > SIGMA_NORM) ? final_cruise_speed : SIGMA_NORM;
vel_sp_along_track = final_cruise_speed;
/* we want to accelerate not too fast
* TODO: change the name acceleration_hor_man to something that can
* be used by auto and manual */
float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
/* 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();
} else {
vel_sp_along_track = final_cruise_speed;
/* we want to accelerate not too fast
* TODO: change the name acceleration_hor_man to something that can
* be used by auto and manual */
float acc_track = (final_cruise_speed - vel_sp_along_track_prev) / dt;
if (acc_track > _acceleration_hor_manual.get()) {
vel_sp_along_track = _acceleration_hor_manual.get() * dt + vel_sp_along_track_prev;
}
if (acc_track > _acceleration_hor_manual.get()) {
vel_sp_along_track = _acceleration_hor_manual.get() * dt + vel_sp_along_track_prev;
}
/* enforce minimum cruise speed */
@ -1831,7 +1856,7 @@ void MulticopterPositionControl::control_auto(float dt)
cruise_sp = get_cruising_speed_xy();
}
/* sanity check: done divide by zero */
/* sanity check: don't divide by zero */
if (vec_pos_to_closest.length() > SIGMA_NORM) {
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);

14
src/modules/mc_pos_control/mc_pos_control_params.c
View File

@ -245,20 +245,20 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
/**
* Minimum horizontal velocity in mission
* Cruise speed when angle prev-current/current-next setpoint
* is 90 degrees. It should be lower than MPC_XY_CRUISE.
*
* Normal horizontal velocity in AUTO modes (includes
* also RTL / hold / etc.) and endpoint for
* position stabilized mode (POSCTRL).
* Applies only in AUTO modes (includes
* also RTL / hold / etc.)
*
* @unit m/s
* @min 0.05
* @max 1.0
* @min 1.0
* @max
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_CRUISE_MIN, 0.5f);
PARAM_DEFINE_FLOAT(MPC_CRUISE_90, 3.0f);
/**
* Maximum horizontal velocity setpoint for manual controlled mode