From 835ab4f10ff4941e68ee4ded91bc7429d68a6d72 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Nov 2015 15:03:28 +0100 Subject: [PATCH] Systemlib: Add USB circuit breaker --- src/modules/systemlib/circuit_breaker.h | 1 + .../systemlib/circuit_breaker_params.c | 13 ++++++++++++ src/modules/systemlib/rc_check.c | 20 ++++++++++--------- 3 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index c76e6c37f3..b4d9b5d1c6 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -56,6 +56,7 @@ #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 #define CBRK_GPSFAIL_KEY 240024 +#define CBRK_USB_CHK_KEY 197848 #include diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 003b73bb62..a3eaebbb33 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -147,3 +147,16 @@ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); * @group Circuit Breaker */ PARAM_DEFINE_INT32(CBRK_BUZZER, 0); + +/** + * Circuit breaker for USB link check + * + * Setting this parameter to 197848 will disable the USB connected + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 197848 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_USB_CHK, 0); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 0f5d96f2b8..3456fafc68 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -74,7 +74,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_t map_parm = param_find(rc_map_mandatory[j]); if (map_parm == PARAM_INVALID) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -87,7 +87,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) param_get(map_parm, &mapping); if (mapping > RC_INPUT_MAX_CHANNELS) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -95,7 +95,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) } if (mapping == 0) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -148,7 +148,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_min < RC_INPUT_LOWEST_MIN_US) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -157,7 +157,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_max > RC_INPUT_HIGHEST_MAX_US) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -166,7 +166,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim < param_min) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -175,7 +175,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (param_trim > param_max) { count++; - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -183,7 +183,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) /* assert deadzone is sane */ if (param_dz > RC_INPUT_MAX_DEADZONE_US) { - if (report_fail) { mavlink_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } + if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); } /* give system time to flush error message in case there are more */ usleep(100000); @@ -200,9 +200,11 @@ int rc_calibration_check(int mavlink_fd, bool report_fail) if (channels_failed) { sleep(2); - if (report_fail) mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", + if (report_fail) { + mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.", total_fail_count, (total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : ""); + } usleep(100000); }