Commander: Fix GPS loss not handled properly

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);