From c0a9e08a30f01026436e80d2be948acf5eb267a6 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 19:18:05 +0200 Subject: [PATCH 01/10] ROMFS: Start GPS driver before Commander so that preflight check can be run --- ROMFS/px4fmu_common/init.d/rcS | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 1237d9bb11..209994ff59 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -301,6 +301,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 @@ -472,22 +484,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 # From 45f1fd663444cbba5cb7304b5ea7eda8fe0af680 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 19:19:30 +0200 Subject: [PATCH 02/10] Commander: Add preflight check for missing GPS module --- src/modules/commander/PreflightCheck.cpp | 26 +++++++++++++++++++++++- src/modules/commander/PreflightCheck.h | 6 +++++- src/modules/commander/commander.cpp | 9 ++++---- 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 4d9bd8ae0c..893586346e 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -61,6 +61,7 @@ #include #include +#include #include @@ -269,8 +270,24 @@ out: return success; } +static bool gnssCheck(int mavlink_fd) +{ + bool success = true; + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps; + + if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) || + (hrt_elapsed_time(&gps.timestamp_position) > 500000)) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + success = false; + } + + 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 +353,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 7fbb0a2dac..806951cf8d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -916,6 +916,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 +1118,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 +1126,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, status.circuit_breaker_engaged_gpsfailure_check, true); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1300,7 +1299,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, status.circuit_breaker_engaged_gpsfailure_check, true); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -2749,7 +2748,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, status.circuit_breaker_engaged_gpsfailure_check, true); arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); From f020ad4ba31017925ace61fb60244f9043e4ce08 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 19:29:35 +0200 Subject: [PATCH 03/10] Commander: Check if GPS receiver is suffering from jamming noise --- src/modules/commander/commander.cpp | 36 ++++++++++++++++++++--------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 806951cf8d..b6deffe75c 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)) @@ -1634,19 +1636,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"); From fee02c6943101287f266eb66a8cf3733e077a0be Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 20:30:10 +0200 Subject: [PATCH 04/10] Commander: Fix parameter bug in preflight check function --- src/modules/commander/commander.cpp | 6 +++--- src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b6deffe75c..c74c620212 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1128,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, status.circuit_breaker_engaged_gpsfailure_check, 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 } @@ -1301,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, status.circuit_breaker_engaged_gpsfailure_check, 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; @@ -2762,7 +2762,7 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, status.circuit_breaker_engaged_gpsfailure_check, 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..990e549a05 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); } From 52222de0213fbe196cc514296472d586ae66befe Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 20:59:23 +0200 Subject: [PATCH 05/10] Commander: Wait up to 1 second to allow GPS module to be detected --- src/modules/commander/PreflightCheck.cpp | 25 +++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 893586346e..f90406cf00 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -273,14 +274,28 @@ out: static bool gnssCheck(int mavlink_fd) { bool success = true; - int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps; - if (!orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps) || - (hrt_elapsed_time(&gps.timestamp_position) > 500000)) { - mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + //Wait up to 1000ms 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, 1000) <= 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; From 510b6124ecc7df6a10cf7ba8b87ca46e0f743e30 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 21:20:05 +0200 Subject: [PATCH 06/10] Commander: Fix inverted circuit breaker logic --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c74c620212..dae51233d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1128,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.circuit_breaker_engaged_gpsfailure_check); + 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 } @@ -1301,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, status.circuit_breaker_engaged_gpsfailure_check); + (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,7 +1637,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS is ok */ - if (status.circuit_breaker_engaged_gpsfailure_check) { + 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 @@ -2762,7 +2762,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.circuit_breaker_engaged_gpsfailure_check); + 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 990e549a05..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, status->circuit_breaker_engaged_gpsfailure_check, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true); } From d650820dbfafbdf17a2688143752223970b1d9d4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Thu, 14 May 2015 21:20:34 +0200 Subject: [PATCH 07/10] SystemLib: Add missing CBRK_GPSFAIL circuit breaker parameter --- src/modules/systemlib/circuit_breaker_params.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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 From 38004cdd955dad01801b750e18e45ac5dd3000e4 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 18 May 2015 12:31:16 +0200 Subject: [PATCH 08/10] PreflightCheck: Increase GPS timeout to 4 sec --- src/modules/commander/PreflightCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index f90406cf00..cc993bc015 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -281,7 +281,7 @@ static bool gnssCheck(int mavlink_fd) struct pollfd fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; - if(poll(fds, 1, 1000) <= 0) { + if(poll(fds, 1, 4000) <= 0) { success = false; } else { From 680898e6aad91ea1de1e832c821c8fa1d3fdee0e Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 18 May 2015 12:48:40 +0200 Subject: [PATCH 09/10] GPS: Publish first data after configuring device --- src/drivers/gps/gps.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) 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) { From cd67609da5755dae6cd81e60bb611e498ff2d180 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Mon, 18 May 2015 12:49:53 +0200 Subject: [PATCH 10/10] PreflightCheck: Reduce GPS timeout to 2 sec --- src/modules/commander/PreflightCheck.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index cc993bc015..96e7757da0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -277,11 +277,11 @@ static bool gnssCheck(int mavlink_fd) int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); - //Wait up to 1000ms to allow the driver to detect a GNSS receiver module + //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, 4000) <= 0) { + if(poll(fds, 1, 2000) <= 0) { success = false; } else {