fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 21:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: Update prev and next waypoint depending on validity

Browse Source 
This fixes the corner case where a NAV_DELAY command changes the
validity of the next WP but not the rest of the triplet. The logic in
FlightTask was ignoring this change because the check was only based on
WP position change.
This commit is contained in:
bresch
2020-06-26 09:48:22 -04:00
committed by Daniel Agar
parent 3302a6965d
commit 1761ef3952
2 changed files with 10 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
+8 -1
View File
@@ -195,13 +195,16 @@ bool FlightTaskAuto::_evaluateTriplets()
// TODO This is a hack and it would be much better if the navigator only sends out a waypoints once they have changed.
bool triplet_update = true;
const bool prev_next_validity_changed = (_prev_was_valid != _sub_triplet_setpoint.get().previous.valid)
|| (_next_was_valid != _sub_triplet_setpoint.get().next.valid);
if (PX4_ISFINITE(_triplet_target(0))
&& PX4_ISFINITE(_triplet_target(1))
&& PX4_ISFINITE(_triplet_target(2))
&& fabsf(_triplet_target(0) - tmp_target(0)) < 0.001f
&& fabsf(_triplet_target(1) - tmp_target(1)) < 0.001f
&& fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f) {
&& fabsf(_triplet_target(2) - tmp_target(2)) < 0.001f
&& !prev_next_validity_changed) {
// Nothing has changed: just keep old waypoints.
triplet_update = false;
@@ -231,6 +234,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_triplet_prev_wp = _position;
}
_prev_was_valid = _sub_triplet_setpoint.get().previous.valid;
if (_type == WaypointType::loiter) {
_triplet_next_wp = _triplet_target;
@@ -242,6 +247,8 @@ bool FlightTaskAuto::_evaluateTriplets()
} else {
_triplet_next_wp = _triplet_target;
}
_next_was_valid = _sub_triplet_setpoint.get().next.valid;
}
if (_ext_yaw_handler != nullptr) {