fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 14:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator_main: allow altitude change when the current lat and lon are still nan

Browse Source 
This is a corner case that happens when the user wants to command an
altitude change just after a takeoff. Takeoff doesn't set the current
lat and lon therefore the user had to change altitude twice to get it working.
The first time to set the current lat and lon and the second time to
actually change altitude.
This commit is contained in:
daniele
2020-04-08 21:16:31 -04:00
committed by Daniel Agar
parent 6ea60b5165
commit 38e635e386
1 changed files with 11 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+11 -5
View File
@@ -300,13 +300,19 @@ Navigator::run()
rep->current.alt = get_global_position()->alt;
}
} else if (PX4_ISFINITE(cmd.param7) && curr->current.valid
&& PX4_ISFINITE(curr->current.lat)
&& PX4_ISFINITE(curr->current.lon)) {
} else if (PX4_ISFINITE(cmd.param7)) {
// Altitude without position change
rep->current.lat = curr->current.lat;
rep->current.lon = curr->current.lon;
// This condition is necessary for altitude changes just after takeoff where lat and lon are still nan
if (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;
} else {
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
}
rep->current.alt = cmd.param7;
} else {