fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 02:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

FlightTaskAuto: always update yaw

Browse Source
This commit is contained in:
Dennis Mannhart
2018-03-23 17:18:14 +01:00
committed by Lorenz Meier
parent 29391acbca
commit 1cde38f82f
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/FlightTaskAuto.cpp
+8 -8
View File
@@ -111,6 +111,13 @@ bool FlightTaskAuto::_evaluateTriplets()
target(2) = -(_sub_triplet_setpoint->get().current.alt - _reference_altitude);
_yaw_wp = _sub_triplet_setpoint->get().current.yaw;
if (!PX4_ISFINITE(_yaw_wp)) {
_yaw_wp = _yaw;
}
/* Check if anything has changed. We do that by comparing the target
* setpoint to the previous target.
* TODO This is a hack and it would be much
@@ -119,7 +126,7 @@ bool FlightTaskAuto::_evaluateTriplets()
/* Dont't do any updates if the current target has not changed */
if (!(fabsf(target(0) - _target(0)) > 0.001f || fabsf(target(1) - _target(1)) > 0.001f
|| fabsf(target(2) - _target(2)) > 0.001f || fabsf(_sub_triplet_setpoint->get().current.yaw - _yaw_wp) > 0.001f)) {
|| fabsf(target(2) - _target(2)) > 0.001f)) {
/* Nothing has changed: just keep old waypoints */
return true;
}
@@ -137,13 +144,6 @@ bool FlightTaskAuto::_evaluateTriplets()
_target(2) = _position(2);
}
_yaw_wp = _sub_triplet_setpoint->get().current.yaw;
if (!PX4_ISFINITE(_yaw_wp)) {
_yaw_wp = _yaw;
}
_prev_prev_wp = _prev_wp; // previous -1 is set to previous
if (_isFinite(_sub_triplet_setpoint->get().previous) && _sub_triplet_setpoint->get().previous.valid) {