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

navigator: Finish rework of switch distance to account for vehicle dynamics

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-08 16:48:50 +02:00
parent 1fb7434128
commit 1ecbf674aa
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+3 -3
View File
@@ -586,7 +586,7 @@ Mission::altitude_sp_foh_update()
} }
/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */ /* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) { if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) {
return; return;
} }
@@ -608,7 +608,7 @@ Mission::altitude_sp_foh_update()
/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
* navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) { if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
pos_sp_triplet->current.alt = _mission_item.altitude; pos_sp_triplet->current.alt = _mission_item.altitude;
} else { } else {
/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp /* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
@@ -617,7 +617,7 @@ Mission::altitude_sp_foh_update()
* radius around the current waypoint * radius around the current waypoint
**/ **/
float delta_alt = (_mission_item.altitude - _mission_item_previous_alt); float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius); float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
float a = _mission_item_previous_alt - grad * _distance_current_previous; float a = _mission_item_previous_alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;