diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e100e6b4b7..2dfe3d8e30 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/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 # diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 714c80ded2..e3706cf671 100644 --- a/src/drivers/gps/gps.cpp +++ b/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) { - - 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); - } + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 4d9bd8ae0c..96e7757da0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -61,6 +62,7 @@ #include #include +#include #include @@ -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; } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 66947a3470..b6423a4d9a 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9676ad53e2..5969de6e83 100644 --- a/src/modules/commander/commander.cpp +++ b/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 */ - if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - 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"); + /* check if GPS is ok */ + if (!status.circuit_breaker_engaged_gpsfailure_check) { + bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE; + + //Check if GPS receiver is too noisy while we are disarmed + if (!armed.armed && gpsIsNoisy) { + if (!status.gps_failure) { + mavlink_log_critical(mavlink_fd, "GPS signal noisy"); + 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 (!status.gps_failure) { + if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* 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); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 73fdb09406..cdcc12043e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/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); } diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e5cc034bc9..097903d21f 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/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); \ No newline at end of file