From 1ecbf674aaffdbc6cfa7e3f30959283c9e8dba3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 7 Jun 2015 11:51:06 +0200 Subject: [PATCH] navigator: Finish rework of switch distance to account for vehicle dynamics --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e36e3fa386..98b01c7578 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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 */ - 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; } @@ -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 * 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; } else { /* 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 **/ 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; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;