Merge pull request #2175 from Zefz/preflight_gps_check

Preflight: GPS check

ROMFS/px4fmu_common/init.d/rcS

@@ -302,6 +302,18 @@ then
 	# Sensors System (start before Commander so Preflight checks are properly run)
 	#
 	sh /etc/init.d/rc.sensors
+	
+	if [ $GPS == yes ]
+	then
+		if [ $GPS_FAKE == yes ]
+		then
+			echo "[i] Faking GPS"
+			gps start -f
+		else
+			gps start
+		fi
+	fi
+	unset GPS_FAKE
 
 	# Needs to be this early for in-air-restarts
 	commander start
@@ -479,22 +491,10 @@ then
 	sh /etc/init.d/rc.uavcan
 
 	#
-	# Logging, GPS
+	# Logging
 	#
 	sh /etc/init.d/rc.logging
 
-	if [ $GPS == yes ]
-	then
-		if [ $GPS_FAKE == yes ]
-		then
-			echo "[i] Faking GPS"
-			gps start -f
-		else
-			gps start
-		fi
-	fi
-	unset GPS_FAKE
-
 	#
 	# Start up ARDrone Motor interface
 	#

src/drivers/gps/gps.cpp

@@ -354,6 +354,20 @@ GPS::task_main()
 			if (_Helper->configure(_baudrate) == 0) {
 				unlock();
 
+				//Publish initial report that we have access to a GPS
+				//Make sure to clear any stale data in case driver is reset
+				memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
+				_report_gps_pos.timestamp_position = hrt_absolute_time();
+				_report_gps_pos.timestamp_variance = hrt_absolute_time();
+				_report_gps_pos.timestamp_velocity = hrt_absolute_time();
+				_report_gps_pos.timestamp_time = hrt_absolute_time();
+				if (_report_gps_pos_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+
+				} else {
+					_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+				}
+
 				// GPS is obviously detected successfully, reset statistics
 				_Helper->reset_update_rates();
 
@@ -364,13 +378,7 @@ GPS::task_main()
 
 					if (!(_pub_blocked)) {
 						if (helper_ret & 1) {
-
+							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-							if (_report_gps_pos_pub > 0) {
-								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
-
-							} else {
-								_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
-							}
 						}
 						if (_p_report_sat_info && (helper_ret & 2)) {
 							if (_report_sat_info_pub > 0) {

src/modules/commander/PreflightCheck.cpp

@@ -48,6 +48,7 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <math.h>
+#include <poll.h>
 
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
@@ -61,6 +62,7 @@
 #include <drivers/drv_airspeed.h>
 
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_gps_position.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -269,8 +271,38 @@ out:
 	return success;
 }
 
+static bool gnssCheck(int mavlink_fd)
+{
+	bool success = true;
+
+	int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	//Wait up to 2000ms 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, 2000) <= 0) {
+		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);
+	return success;
+}
+
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
-		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic)
+		    bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic)
 {
 	bool failed = false;
 
@@ -336,6 +368,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 		}
 	}
 
+	/* ---- Global Navigation Satellite System receiver ---- */
+	if(checkGNSS) {
+		if(!gnssCheck(mavlink_fd)) {
+			failed = true;
+		}
+	}
+
 	/* Report status */
 	return !failed;
 }

src/modules/commander/PreflightCheck.h

@@ -59,11 +59,15 @@ namespace Commander
 *   true if the gyroscopes should be checked
 * @param checkBaro
 *   true if the barometer should be checked
+* @param checkAirspeed
+*   true if the airspeed sensor should be checked
 * @param checkRC
 *   true if the Remote Controller should be checked
+* @param checkGNSS
+*   true if the GNSS receiver should be checked
 **/
 bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
-	bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false);
+    bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false);
 
 const unsigned max_mandatory_gyro_count = 1;
 const unsigned max_optional_gyro_count = 3;

src/modules/commander/commander.cpp

@@ -125,6 +125,8 @@ static const int ERROR = -1;
 
 extern struct system_load_s system_load;
 
+static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage signal to noise ratio allowed for GPS reception */
+
 /* Decouple update interval and hysteris counters, all depends on intervals */
 #define COMMANDER_MONITORING_INTERVAL 50000
 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
@@ -916,6 +918,7 @@ int commander_thread_main(int argc, char *argv[])
 	status.circuit_breaker_engaged_airspd_check = false;
 	status.circuit_breaker_engaged_enginefailure_check = false;
 	status.circuit_breaker_engaged_gpsfailure_check = false;
+	get_circuit_breaker_params();
 
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -1117,8 +1120,6 @@ int commander_thread_main(int argc, char *argv[])
 	param_get(_param_sys_type, &(status.system_type)); // get system type
 	status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
 
-	get_circuit_breaker_params();
-
 	bool checkAirspeed = false;
 	/* Perform airspeed check only if circuit breaker is not
 	 * engaged and it's not a rotary wing */
@@ -1127,7 +1128,7 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	// Run preflight check
-	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 	if (!status.condition_system_sensors_initialized) {
 		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
 	}
@@ -1300,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 					/* provide RC and sensor status feedback to the user */
-					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true);
+					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 				}
 
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -1637,19 +1638,31 @@ int commander_thread_main(int argc, char *argv[])
 						  (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
 		}
 
-		/* check if GPS fix is ok */
+		/* check if GPS is ok */
-		if (status.circuit_breaker_engaged_gpsfailure_check ||
+		if (!status.circuit_breaker_engaged_gpsfailure_check) {
-		    (gps_position.fix_type >= 3 &&
+			bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE;
-		     hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) {
+
-			/* handle the case where gps was regained */
+			//Check if GPS receiver is too noisy while we are disarmed
-			if (status.gps_failure) {
+			if (!armed.armed && gpsIsNoisy) {
-				status.gps_failure = false;
+				if (!status.gps_failure) {
-				status_changed = true;
+					mavlink_log_critical(mavlink_fd, "GPS signal noisy");
-				mavlink_log_critical(mavlink_fd, "gps regained");
+					set_tune_override(TONE_GPS_WARNING_TUNE);
+
+					//GPS suffers from signal jamming or excessive noise, disable GPS-aided flight
+					status.gps_failure = true;
+					status_changed = true;
+				}
 			}
 
-		} else {
+			if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) {
-			if (!status.gps_failure) {
+				/* handle the case where gps was regained */
+				if (status.gps_failure && !gpsIsNoisy) {
+					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");
@@ -2751,7 +2764,7 @@ void *commander_low_prio_loop(void *arg)
 							checkAirspeed = true;
 						}
 
-						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
+						status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check);
 
 						arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 

src/modules/commander/state_machine_helper.cpp

@@ -689,5 +689,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		checkAirspeed = true;
 	}
 
-	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
+	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true);
 }

src/modules/systemlib/circuit_breaker_params.c

@@ -120,3 +120,19 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
  * @group Circuit Breaker
  */
 PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
+
+/**
+ * Circuit breaker for GPS failure detection
+ *
+ * Setting this parameter to 240024 will disable the GPS failure detection.
+ * If this check is enabled, then the sensor check will fail if the GPS module
+ * is missing. It will also check for excessive signal noise on the GPS receiver
+ * and warn the user if detected.
+ * 
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 240024
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);