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

commander: move position of gps failure check

Browse Source
This commit is contained in:
Thomas Gubler
2014-09-21 13:32:47 +02:00
parent bdddcf338d
commit 980175bb60
1 changed files with 17 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+17 -18
View File
@@ -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_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 */ /* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { (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()); 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 */ /* start mission result check */
orb_check(mission_result_sub, &updated); orb_check(mission_result_sub, &updated);