diff options
author | Julian Oes <julian@oes.ch> | 2014-09-22 06:35:36 +1000 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-09-22 06:35:36 +1000 |
commit | 323b6f10c3b35c7f688255c2434738b1530102f6 (patch) | |
tree | a5a801c0f0f55c917bf0a7abfce223a4dc2dffa2 | |
parent | e65ec1b98b6f0ed4d526b2d9d1929d64fd7eceda (diff) | |
parent | 980175bb60ba40d29091f92fa8d494aed79b9ef6 (diff) | |
download | px4-firmware-323b6f10c3b35c7f688255c2434738b1530102f6.tar.gz px4-firmware-323b6f10c3b35c7f688255c2434738b1530102f6.tar.bz2 px4-firmware-323b6f10c3b35c7f688255c2434738b1530102f6.zip |
Merge pull request #18 from swissfang/gpschecklocation
commander: move position of gps failure check
-rw-r--r-- | src/modules/commander/commander.cpp | 35 |
1 files changed, 17 insertions, 18 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b14349..03b73447f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); |