fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_pos_control: remove scaling in auto and remove limit_velocity function; update auto logic

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent c82deaf26f
commit a7be9854d9
2 changed files with 95 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+77 -89
View File
@@ -144,6 +144,7 @@ private:
control::BlockParamFloat _acceleration_hor_max; /**< maximum velocity setpoint slewrate while decelerating */
control::BlockParamFloat _deceleration_hor_max; /**< maximum velocity setpoint slewrate while decelerating */
control::BlockParamFloat _target_threshold_xy; /**< distance threshold for slowdown close to target during mission */
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 */
@@ -407,6 +408,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
_target_threshold_xy(this, "TARGET_THRE"),
_velocity_hor_manual(this, "VEL_MAN_MAX", true),
_takeoff_ramp_time(this, "TKO_RAMP_T", true),
_min_cruise_speed(this, "CRUISE_MIN", true),
_vel_x_deriv(this, "VELD"),
_vel_y_deriv(this, "VELD"),
_vel_z_deriv(this, "VELD"),
@@ -862,25 +864,6 @@ MulticopterPositionControl::limit_altitude()
}
}
void
MulticopterPositionControl::limit_vel_xy_gradually()
{
/*
* the max velocity is defined by the linear line
* with x= (curr_sp - pos) and y = _vel_sp with min limit of 0.01
*/
math::Vector<3> dist = _curr_pos_sp - _pos;
float slope = (get_cruising_speed_xy() - 0.01f) / _target_threshold_xy.get();
float vel_limit = slope * sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) + 0.01f;
float vel_mag_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
if (vel_mag_xy <= vel_limit) {
return;
}
_vel_sp(0) = _vel_sp(0) / vel_mag_xy * vel_limit;
_vel_sp(1) = _vel_sp(1) / vel_mag_xy * vel_limit;
}
bool
MulticopterPositionControl::in_auto_takeoff()
@@ -1285,10 +1268,17 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
/* project center of sphere on line */
/* normalized AB */
math::Vector<3> ab_norm = line_b - line_a;
if (ab_norm.length() < 0.01f) {
return true;
}
ab_norm.normalize();
math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
float cd_len = (sphere_c - d).length();
//PX4_INFO("sphere_r: %.5f",(double)sphere_r);
//PX4_INFO("cd len: %.5f",(double)cd_len);
if (sphere_r > cd_len) {
/* we have triangle CDX with known CD and CX = R, find DX */
float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
@@ -1305,16 +1295,21 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
return true;
} else {
PX4_INFO("return false");
/* have no roots, return D */
res = d; /* go directly to line */
/* previous waypoint is still in front of us */
if ((sphere_c - line_a) * ab_norm < 0.0f) {
PX4_INFO("previous is in front");
res = line_a;
}
/* target waypoint is already behind us */
if ((sphere_c - line_b) * ab_norm > 0.0f) {
PX4_INFO("target is behind");
res = line_b;
}
@@ -1405,11 +1400,6 @@ void MulticopterPositionControl::control_auto(float dt)
}
}
/* set velocity limit if close to current setpoint and no next setpoint available */
math::Vector<3> dist = _curr_pos_sp - _pos;
_limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
&& (sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) <= _target_threshold_xy.get());
if (current_setpoint_valid &&
(_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {
@@ -1419,92 +1409,100 @@ void MulticopterPositionControl::control_auto(float dt)
/* scaled space: 1 == position error resulting max allowed speed */
math::Vector<3> cruising_speed(cruising_speed_xy, cruising_speed_xy, cruising_speed_z);
/* if previous is valid, we want to follow line */
if (previous_setpoint_valid
&& (_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)) {
math::Vector<3> scale = _params.pos_p.edivide(cruising_speed);
/* convert current setpoint to scaled space */
math::Vector<3> curr_sp_s = _curr_pos_sp.emult(scale);
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
math::Vector<3> pos_sp = _curr_pos_sp;
const float minimum_dist = 0.01f;
if ((_curr_pos_sp - prev_sp).length() > minimum_dist) {
/* find X - cross point of unit sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
/* unit vector from previous to current */
matrix::Vector2f unit_prev_to_current((_curr_pos_sp(0) - prev_sp(0)), (_curr_pos_sp(1) - prev_sp(1)));
unit_prev_to_current.normalize();
/* we are close to current setpoint */
if (curr_pos_s_len < 1.0f) {
/* orthogonal distance from current position to unit_prev_to_current */
matrix::Vector2f closest_point = matrix::Vector2f(prev_sp(0), prev_sp(1)) + unit_prev_to_current *
(matrix::Vector2f((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1))) * unit_prev_to_current);
/* if next is valid, we want to have smooth transition */
if (next_setpoint_valid && (next_sp - _curr_pos_sp).length() > minimum_dist) {
/* check if we need to adjust position setpoint based on cruise velocity */
matrix::Vector2f vec_pos_to_current = matrix::Vector2f((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1)));
matrix::Vector2f vec_prev_to_pos = matrix::Vector2f((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1)));
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* slow down */
if (vec_pos_to_current.length() < _target_threshold_xy.get()) {
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/* angle prev-current-next defines speed close to waypoint */
if (next_setpoint_valid) {
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
/* unit vector to 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;
/* cos(b), b = angle pos - _curr_pos_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
/* 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;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* velocity close to target adjusted to angle
* TODO: add explanation*/
float M = get_cruising_speed_xy() / 4.0f;
float a = -((M - get_cruising_speed_xy()) * (M - get_cruising_speed_xy())) / (2.0f * M - get_cruising_speed_xy() -
_min_cruise_speed.get());
float c = get_cruising_speed_xy() - a;
float b = (M - c) / a;
float vel_close = a * powf(b, angle) + c;
/* if curr - next distance is larger than unit radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
}
float slope = (get_cruising_speed_xy() - vel_close) / _target_threshold_xy.get();
cruising_speed_xy = slope * vec_pos_to_current.length() + vel_close;
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
} else {
/* we want to stop at next waypoint */
float slope = (get_cruising_speed_xy() - _min_cruise_speed.get()) / _target_threshold_xy.get();
cruising_speed_xy = slope * vec_pos_to_current.length() + _min_cruise_speed.get();
}
} else {
/* if not close to current setpoint, check if we are within cross_sphere_line */
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (!near) {
/* we're far away from trajectory, pos_sp_s is set to the nearest point on the trajectory */
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
/* accelerate */
} else if (vec_prev_to_pos.length() < _target_threshold_xy.get()) {
float slope = (get_cruising_speed_xy() - _min_cruise_speed.get()) / _target_threshold_xy.get();
cruising_speed_xy = slope * vec_prev_to_pos.length() + _min_cruise_speed.get();
}
/* we want position setpoint not farther away then cruise speed */
if (vec_pos_to_current.length() * _params.pos_p(0) > cruising_speed_xy) {
/* we adjust position setpoint */
matrix::Vector2f vec_pos_to_closest = closest_point - matrix::Vector2f(_pos(0), _pos(1));
float vel_orthogonal = vec_pos_to_closest.length() * _params.pos_p(0);
if (vel_orthogonal <= cruising_speed_xy) {
float vel_along_track = sqrtf(cruising_speed_xy * cruising_speed_xy - vel_orthogonal * vel_orthogonal);
pos_sp(0) = closest_point(0) + unit_prev_to_current(0) * vel_along_track / _params.pos_p(0);
pos_sp(1) = closest_point(1) + unit_prev_to_current(1) * vel_along_track / _params.pos_p(0);
} else {
pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruising_speed_xy / _params.pos_p(0);
pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruising_speed_xy / _params.pos_p(1);
}
}
}
/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
/* scale back */
_pos_sp = pos_sp_s.edivide(scale);
_pos_sp = pos_sp;
} else {
/* we just have a current setpoint that we want to go to */
/* we just have a current setpoint that we want to go to */;
_pos_sp = _curr_pos_sp;
/* set max velocity to cruise */
@@ -1620,11 +1618,6 @@ MulticopterPositionControl::do_control(float dt)
_run_pos_control = true;
_run_alt_control = true;
/* if not in auto mode, we reset limit_vel_xy flag */
if (_control_mode.flag_control_manual_enabled || _control_mode.flag_control_offboard_enabled) {
_limit_vel_xy = false;
}
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
@@ -1689,11 +1682,6 @@ MulticopterPositionControl::calculate_velocity_setpoint(float dt)
}
}
/* we are close to target and want to limit velocity in xy */
if (_limit_vel_xy) {
limit_vel_xy_gradually();
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
}
src/modules/mc_pos_control/mc_pos_control_params.c
+18 -2
View File
@@ -229,7 +229,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f);
PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
/**
* Nominal horizontal velocity in mission
* Maximum horizontal velocity in mission
*
* Normal horizontal velocity in AUTO modes (includes
* also RTL / hold / etc.) and endpoint for
@@ -245,7 +245,23 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f);
/**
* Nominal horizontal velocity for manual controlled mode
* Minimum horizontal velocity in mission
*
* Normal horizontal velocity in AUTO modes (includes
* also RTL / hold / etc.) and endpoint for
* position stabilized mode (POSCTRL).
*
* @unit m/s
* @min 0.5
* @max 8.0
* @increment 1
* @decimal 2
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_CRUISE_MIN, 1.0f);
/**
* Maximum horizontal velocity setpoint for manual controlled mode
*
* @unit m/s
* @min 3.0