dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

i.e. the altitude reference oscillations caused by it in LOITER mode
This commit is contained in:
Philipp Oettershagen 2018-07-16 05:54:08 +02:00 committed by Daniel Agar
parent b0f766d90e
commit ab044e274d
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/navigator/mission.cpp
View File

@ -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;
}
@ -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 {
@ -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;
}