fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 11:22:02 +02:00 committed by Daniel Agar
parent 3302a6965d
commit 1761ef3952
2 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp
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) {

2
src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.hpp
View File

@ -100,8 +100,10 @@ protected:
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
bool _prev_was_valid{false};
matrix::Vector3f _target{}; /**< Target waypoint (local frame).*/
matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
bool _next_was_valid{false};
float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */