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, 27 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 */