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

mc_pos_control auto: treat off the track differently depending on position

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent a7be9854d9
commit 2735280ffc
1 changed files with 27 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+27 -14
View File
@@ -1277,8 +1277,6 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
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);
@@ -1296,20 +1294,16 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
} 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;
}
@@ -1426,15 +1420,15 @@ void MulticopterPositionControl::control_auto(float dt)
/* 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();
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_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);
/* 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)));
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_sp(0)), (_pos(1) - prev_sp(1)));
/* slow down */
if (vec_pos_to_current.length() < _target_threshold_xy.get()) {
@@ -1448,10 +1442,12 @@ void MulticopterPositionControl::control_auto(float dt)
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;
float angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f;
/* velocity close to target adjusted to angle
* TODO: add explanation*/
* 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
* the velocity at target should be 1/4 * cruising speed;
* angle = 2 -> vel_close = min_cruising_speed */
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());
@@ -1469,10 +1465,8 @@ void MulticopterPositionControl::control_auto(float dt)
cruising_speed_xy = slope * vec_pos_to_current.length() + _min_cruise_speed.get();
}
/* accelerate */
} else if (vec_prev_to_pos.length() < _target_threshold_xy.get()) {
/* accelerate */
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();
}
@@ -1485,12 +1479,31 @@ void MulticopterPositionControl::control_auto(float dt)
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);
/* we split position error vector into along track and orthogonal such that cruise speed is never exceeded */
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 {
/* we are more then cruisespeed away from track */
/* check on which section we are with default closest point */
if ((vec_prev_to_pos * unit_prev_to_current) < 0.0f) {
/* previous is in front */
vec_pos_to_closest(0) = prev_sp(0);
vec_pos_to_closest(1) = prev_sp(1);
}
if (((vec_pos_to_current * -1.0f) * unit_prev_to_current) > 0.0f) {
/* we already passed current_sp */
vec_pos_to_closest(0) = _curr_pos_sp(0);
vec_pos_to_closest(1) = _curr_pos_sp(1);
/* we dont want to limit cruise velocity */
cruising_speed_xy = get_cruising_speed_xy();
}
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);