From dc522f217f5122dcbdc157d5ddb43052e9cb3d3d Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 11 Feb 2015 16:42:01 +0100 Subject: Commander: Fix GPS loss not handled properly --- src/modules/commander/commander.cpp | 35 +++++++++++++----------------- src/modules/commander/commander_helper.cpp | 5 +++++ src/modules/commander/commander_helper.h | 1 + 3 files changed, 21 insertions(+), 20 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f2e72dd0c..b2d9c9da3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1388,30 +1388,25 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* update condition_global_position_valid */ - /* hysteresis for EPH/EPV */ - bool eph_good; - - if (status.condition_global_position_valid) { - if (global_position.eph > eph_threshold * 2.5f) { - eph_good = false; - - } else { - eph_good = true; + //update condition_global_position_valid + //Global positions are only published by the estimators if they are valid + if(hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { + //We have had no good fix for POSITION_TIMEOUT amount of time + if(status.condition_global_position_valid) { + set_tune_override(TONE_GPS_WARNING_TUNE); + status_changed = true; } - } else { - if (global_position.eph < eph_threshold) { - eph_good = true; - - } else { - eph_good = false; + status.condition_global_position_valid = false; + } + else if(hrt_absolute_time() > POSITION_TIMEOUT) { + //Got good global position estimate + if(!status.condition_global_position_valid) { + status_changed = true; + status.condition_global_position_valid = true; } } - check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), - &status_changed); - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -2119,7 +2114,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu /* vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL handled as vehicle_status_s::ARMING_STATE_ARMED_ERROR / vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ } else { - if (status_local->condition_local_position_valid) { + if (status_local->condition_global_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 60e8be23e..b5ec6c4e6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -115,6 +115,11 @@ void buzzer_deinit() close(buzzer); } +void set_tune_override(int tune) +{ + ioctl(buzzer, TONE_SET_ALARM, tune); +} + void set_tune(int tune) { unsigned int new_tune_duration = tune_durations[tune]; diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 4a77fe487..cd3db7324 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -55,6 +55,7 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); +void set_tune_override(int tune); void set_tune(int tune); void tune_positive(bool use_buzzer); void tune_neutral(bool use_buzzer); -- cgit v1.2.3