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

Navigator: Fix pause mode to not continue to next waypoint if already set

This commit is contained in:
Lorenz Meier 2017-07-11 13:23:12 +02:00
parent 8f391a6eae
commit dbc7d805f2
1 changed files with 14 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/navigator/navigator_main.cpp
View File

@ -403,25 +403,32 @@ Navigator::task_main()
rep->current.yaw = NAN;
}
// Position change with optional altitude change
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;
} else if (curr->current.valid
if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}
// Altitude without position change
} else if (PX4_ISFINITE(cmd.param7) && curr->current.valid
&& PX4_ISFINITE(curr->current.lat)
&& PX4_ISFINITE(curr->current.lon)) {
rep->current.lat = curr->current.lat;
rep->current.lon = curr->current.lon;
rep->current.alt = cmd.param7;
// All three set to NaN - hold in current position
} else {
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
}
if (PX4_ISFINITE(cmd.param7)) {
rep->current.alt = cmd.param7;
} else {
rep->current.alt = get_global_position()->alt;
}