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

3 files changed, 51 insertions, 0 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c0628a16..55c74fdff 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -76,6 +76,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -717,6 +718,9 @@ int commander_thread_main(int argc, char *argv[])
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
 
+	status.condition_power_input_valid = true;
+	status.avionics_power_rail_voltage = -1.0f;
+
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
 
@@ -853,6 +857,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct position_setpoint_triplet_s pos_sp_triplet;
 	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 
+	/* Subscribe to system power */
+	int system_power_sub = orb_subscribe(ORB_ID(system_power));
+	struct system_power_s system_power;
+	memset(&system_power, 0, sizeof(system_power));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -933,6 +942,24 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
 		}
 
+		orb_check(system_power_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
+
+			if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
+				if (system_power.servo_valid &&
+					!system_power.brick_valid &&
+					!system_power.usb_connected) {
+					/* flying only on servo rail, this is unsafe */
+					status.condition_power_input_valid = false;
+				} else {
+					status.condition_power_input_valid = true;
+					status.avionics_power_rail_voltage = system_power.voltage5V_v;
+				}
+			}
+		}
+
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
 		/* update safety topic */
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;
 			}
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index ea20a317a..2ed0d6ad4 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -179,6 +179,8 @@ struct vehicle_status_s {
 	bool condition_local_altitude_valid;
 	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
+	bool condition_power_input_valid;		/**< set if input power is valid */
+	float avionics_power_rail_voltage;		/**< voltage of the avionics power rail */
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */