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

mc_pos_control: auto remove min dist

Browse Source
This commit is contained in:
Dennis Mannhart
2017-09-11 02:08:22 -07:00
committed by Lorenz Meier
parent 8c5b1d33da
commit 3053b24761
1 changed files with 9 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+9 -12
View File
@@ -1418,8 +1418,7 @@ void MulticopterPositionControl::control_auto(float dt)
/* Auto logic:
* The vehicle should follow the line previous-current.
* - if there is no next setpoint or the current is a loiter point, then slowly approach the current along the line when close to it until the vehicle is within min_dist_to_current
* where the position setpoint is locked at current
* - if there is no next setpoint or the current is a loiter point, then slowly approach the current along the line
* - if there is a next setpoint, then the velocity is adjusted depending on the angle of the corner prev-current-next.
* When following the line, the pos_sp is computed from the orthogonal distance to the closest point on line and the desired cruise speed along the track.
*/
@@ -1441,11 +1440,12 @@ void MulticopterPositionControl::control_auto(float dt)
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_pos_to_current((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1)));
float min_dist_to_current = 0.5f;
/* check if we just want to stay at current position */
bool stay_at_current_pos = (vec_pos_to_current.length() < min_dist_to_current) &&
(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER || !next_setpoint_valid);
matrix::Vector2f pos_sp_diff((_curr_pos_sp(0) - _pos_sp(0)), (_curr_pos_sp(1) - _pos_sp(1)));
bool stay_at_current_pos = (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER
|| !next_setpoint_valid)
&& ((pos_sp_diff.length()) < 0.001f);
/* only follow line if previous to current has a minimum distance */
if (unit_prev_to_current.length() > 0.1f && !stay_at_current_pos) {
@@ -1501,7 +1501,7 @@ void MulticopterPositionControl::control_auto(float dt)
/* compute the velocity along the track depending on distance close to previous setpoint*/
float slope = (get_cruising_speed_xy() - min_cruise_speed) / _target_threshold_xy.get();
vel_sp_along_track = slope * (vec_prev_to_pos.length() - min_dist_to_current) + min_cruise_speed;
vel_sp_along_track = slope * (vec_prev_to_pos.length()) + min_cruise_speed;
} else if (vec_pos_to_current.length() < _target_threshold_xy.get()) {
/* slow down when close to current setpoint */
@@ -1554,17 +1554,14 @@ 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() - min_dist_to_current) + vel_close;
vel_sp_along_track = slope * (vec_pos_to_current.length()) + vel_close;
} else {
/* we want to stop at current setpoint */
vel_sp_along_track = vec_pos_to_current.length() * _params.pos_p(0);
float slope = (get_cruising_speed_xy()) / _target_threshold_xy.get();
vel_sp_along_track = slope * (vec_pos_to_current.length());
if (vec_pos_to_current.length() > min_dist_to_current) {
float slope = (get_cruising_speed_xy() - _min_cruise_speed.get()) / _target_threshold_xy.get();
vel_sp_along_track = slope * (vec_pos_to_current.length() - min_dist_to_current) + _min_cruise_speed.get();
}
}
}