commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail

1 files changed, 22 insertions, 0 deletions

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 87309b238..818974648 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -144,6 +144,28 @@ 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) {
+
+					if (mavlink_fd) {
+						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+					}
+
+					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)) {
+
+					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;
+				}
+
 			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
 				new_arming_state = ARMING_STATE_STANDBY_ERROR;
 			}