commander: put circuit breaker into effect

2 files changed, 25 insertions, 14 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 55c74fdff..71067ac4f 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -52,6 +52,7 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <systemlib/err.h>
+#include <systemlib/circuit_breaker.h>
 #include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
@@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[])
 	status.condition_power_input_valid = true;
 	status.avionics_power_rail_voltage = -1.0f;
 
+	// CIRCUIT BREAKERS
+	status.circuit_breaker_engaged_power_check = false;
+
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
 
@@ -907,6 +911,9 @@ int commander_thread_main(int argc, char *argv[])
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
+
+				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+
 				status_changed = true;
 
 				/* re-check RC calibration */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 818974648..84f4d03af 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 					valid_transition = false;
 				}
 
-				// Fail transition if power is not good
-				if (!status->condition_power_input_valid) {
+				// Perform power checks only if circuit breaker is not
+				// engaged for these checks
+				if (!status->circuit_breaker_engaged_power_check) {
+					// Fail transition if power is not good
+					if (!status->condition_power_input_valid) {
+
+						if (mavlink_fd) {
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+						}
 
-					if (mavlink_fd) {
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+						valid_transition = false;
 					}
 
-					valid_transition = false;
-				}
+					// Fail transition if power levels on the avionics rail
+					// are insufficient
+					if ((status->avionics_power_rail_voltage > 0.0f) &&
+						(status->avionics_power_rail_voltage < 4.5f)) {
 
-				// Fail transition if power levels on the avionics rail
-				// are insufficient
-				if ((status->avionics_power_rail_voltage > 0.0f) &&
-					(status->avionics_power_rail_voltage < 4.5f)) {
+						if (mavlink_fd) {
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
+						}
 
-					if (mavlink_fd) {
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage);
+						valid_transition = false;
 					}
-
-					valid_transition = false;
 				}
 
 			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {