Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Navigator: Fix fixed-wing first order altitude hold #9850

Merged
merged 1 commit into from Jul 16, 2018
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
14 changes: 10 additions & 4 deletions src/modules/navigator/mission.cpp
Expand Up @@ -1297,8 +1297,15 @@ Mission::altitude_sp_foh_update()
return;
}

// Calculate acceptance radius, i.e. the radius within which we do not perform a first order hold anymore
float acc_rad = _navigator->get_acceptance_radius(_mission_item.acceptance_radius);

if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
acc_rad = _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f);
}

/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < FLT_EPSILON) {
if (_distance_current_previous - acc_rad < FLT_EPSILON) {
return;
}

Expand Down Expand Up @@ -1327,7 +1334,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 < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
if (_min_current_sp_distance_xy < acc_rad) {
pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);

} else {
Expand All @@ -1337,8 +1344,7 @@ Mission::altitude_sp_foh_update()
* radius around the current waypoint
**/
float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt);
float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius(
_mission_item.acceptance_radius));
float grad = -delta_alt / (_distance_current_previous - acc_rad);
float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous;
pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
}
Expand Down