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

Commander: Wait up to 1 second to allow GPS module to be detected

This commit is contained in:
Johan Jansen 2015-05-14 20:59:23 +02:00
parent fee02c6943
commit 52222de021
1 changed files with 20 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
src/modules/commander/PreflightCheck.cpp
View File

@ -48,6 +48,7 @@
#include <fcntl.h> #include <fcntl.h>
#include <errno.h> #include <errno.h>
#include <math.h> #include <math.h>
#include <poll.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
@ -273,14 +274,28 @@ out:
static bool gnssCheck(int mavlink_fd) static bool gnssCheck(int mavlink_fd)
{ {
bool success = true; bool success = true;
int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
struct vehicle_gps_position_s gps;
if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) || int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
(hrt_elapsed_time(&gps.timestamp_position) > 500000)) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); //Wait up to 1000ms to allow the driver to detect a GNSS receiver module
struct pollfd fds[1];
fds[0].fd = gpsSub;
fds[0].events = POLLIN;
if(poll(fds, 1, 1000) <= 0) {
success = false; success = false;
} }
else {
struct vehicle_gps_position_s gps;
if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) ||
(hrt_elapsed_time(&gps.timestamp_position) > 1000000)) {
success = false;
}
}
//Report failure to detect module
if(!success) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
close(gpsSub); close(gpsSub);
return success; return success;