commander: changes when USB connected

- skip avionics rail voltage check when USB connected
 - skip forced reboot on USB disconnect if circuit breaker set
 - avionics voltage preflight check don't silently fail if system_power unavailble
     - explicitly set supply check circuit breaker (CBRK_SUPPLY_CHK)

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -130,6 +130,7 @@ then
 	param set CAL_MAG_PRIME 197388
 
 	param set CBRK_AIRSPD_CHK 0
+	param set CBRK_SUPPLY_CHK 894281
 
 	param set COM_DISARM_LAND 0.5
 	param set COM_OBL_ACT 2

boards/av/x-v1/init/rc.board_defaults

@@ -3,6 +3,8 @@
 # av_x1-v1 specific board init
 #------------------------------------------------------------------------------
 
+# system_power unavailable
+param set CBRK_SUPPLY_CHK 894281
 
 if [ $AUTOCNF = yes ]
 then

boards/bitcraze/crazyflie/init/rc.board_defaults

@@ -3,6 +3,8 @@
 # Bitcraze Crazyflie specific board sensors init
 #------------------------------------------------------------------------------
 
+# system_power unavailable
+param set CBRK_SUPPLY_CHK 894281
 
 if param compare SYS_AUTOSTART 0
 then

boards/holybro/kakutef7/init/rc.board_defaults

@@ -3,6 +3,9 @@
 # KakuteF7 specific board defaults
 #------------------------------------------------------------------------------
 
+# system_power unavailable
+param set CBRK_SUPPLY_CHK 894281
+
 if param compare SYS_AUTOSTART 0
 then
 	# Select the Generic 250 Racer by default

boards/intel/aerofc-v1/init/rc.board_defaults

@@ -3,6 +3,8 @@
 # intel aero specific board defaults
 #------------------------------------------------------------------------------
 
+# system_power unavailable
+param set CBRK_SUPPLY_CHK 894281
 
 if param compare SYS_AUTOSTART 0
 then

boards/omnibus/f4sd/init/rc.board_defaults

@@ -3,6 +3,8 @@
 # Omnibus F4SD specific board defaults
 #------------------------------------------------------------------------------
 
+# system_power unavailable
+param set CBRK_SUPPLY_CHK 894281
 
 # transition from LPE+Q to Q estimator (2019-06-05)
 if param compare SYS_MC_EST_GROUP 1

posix-configs/bbblue/px4.config

@@ -9,6 +9,8 @@
 uorb start
 param load
 
+param set CBRK_SUPPLY_CHK 894281
+
 # Auto-start script index. Defines the auto-start script used to bootstrap the system.
 # It seems that SYS_AUTOSTART does not work as intended on posix platform.
 # For now, find the corresponding settings, and manually set them in ground control.

posix-configs/bbblue/px4_fw.config

@@ -9,6 +9,8 @@
 uorb start
 param load
 
+param set CBRK_SUPPLY_CHK 894281
+
 # Auto-start script index. Defines the auto-start script used to bootstrap the system.
 # It seems that SYS_AUTOSTART does not work as intended on posix platform.
 # For now, find the corresponding settings, and manually set them in ground control.

posix-configs/eagle/200qx/mainapp.config

@@ -9,6 +9,7 @@ muorb start
 logger start -t -b 200
 # Wait 1s before setting parameters for muorb to initialize
 sleep 1
+param set CBRK_SUPPLY_CHK 894281
 param set MAV_BROADCAST 1
 dataman start
 navigator start

posix-configs/eagle/200qx/px4.config

@@ -1,5 +1,6 @@
 uorb start
 qshell start
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set MAV_TYPE 2
 param set MC_YAW_P 12

posix-configs/eagle/210qc/mainapp.config

@@ -9,6 +9,7 @@ muorb start
 logger start -t -b 200
 # Wait 1s before setting parameters for muorb to initialize
 sleep 1
+param set CBRK_SUPPLY_CHK 894281
 param set MAV_BROADCAST 1
 dataman start
 navigator start

posix-configs/eagle/210qc/px4.config

@@ -1,5 +1,6 @@
 uorb start
 qshell start
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set MAV_TYPE 2
 param set MC_YAW_P 12

posix-configs/eagle/flight/mainapp.config

@@ -9,6 +9,7 @@ muorb start
 logger start -t -b 200
 # Wait 1s before setting parameters for muorb to initialize
 sleep 1
+param set CBRK_SUPPLY_CHK 894281
 param set MAV_BROADCAST 1
 dataman start
 navigator start

posix-configs/eagle/flight/px4.config

@@ -1,5 +1,6 @@
 uorb start
 qshell start
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 sleep 1
 gps start -d /dev/tty-4

posix-configs/eagle/hil/mainapphil.config

@@ -2,6 +2,7 @@ uorb start
 muorb start
 # Wait 1s before setting parameters for muorb to initialize
 sleep 1
+param set CBRK_SUPPLY_CHK 894281
 param set MAV_BROADCAST 1
 mavlink start -x -u 14556
 sleep 1

posix-configs/eagle/hil/px4.config

@@ -1,6 +1,7 @@
 uorb start
 qshell start
 
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set SYS_AUTOCONFIG 1
 param set MAV_TYPE 2

posix-configs/excelsior/mainapp.config

@@ -6,6 +6,7 @@
 uorb start
 muorb start
 logger start -e -t
+param set CBRK_SUPPLY_CHK 894281
 param set MAV_BROADCAST 1
 dataman start
 navigator start

posix-configs/excelsior/px4.config

@@ -1,5 +1,6 @@
 uorb start
 qshell start
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set MAV_TYPE 2
 param set MC_YAW_P 12

posix-configs/ocpoc/px4.config

@@ -10,6 +10,7 @@ if [ -f /root/rootfs/eeprom/parameters ]
 then
 	param load
 fi
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 param set MAV_TYPE 2

posix-configs/rpi/px4.config

@@ -11,6 +11,7 @@ if [ -f eeprom/parameters ]
 then
 	param load
 fi
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 param set MAV_TYPE 2

posix-configs/rpi/px4_fw.config

@@ -11,6 +11,7 @@ if [ -f eeprom/parameters ]
 then
 	param load
 fi
+param set CBRK_SUPPLY_CHK 894281
 param set MAV_BROADCAST 1
 param set SYS_AUTOSTART 2100
 param set MAV_TYPE 1

posix-configs/rpi/px4_hil.config

@@ -13,6 +13,7 @@ if [ -f eeprom/parameters ]
 then
 	param load
 fi
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 1001
 param set MAV_BROADCAST 1
 param set MAV_TYPE 2

posix-configs/rpi/px4_test.config

@@ -11,6 +11,7 @@ if [ -f eeprom/parameters ]
 then
 	param load
 fi
+param set CBRK_SUPPLY_CHK 894281
 param set SYS_AUTOSTART 4001
 param set MAV_BROADCAST 1
 param set MAV_TYPE 2

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -65,9 +65,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 			  && !status_flags.condition_calibration_enabled);
 
 	const bool checkSensors = !hil_enabled;
-	const bool checkRC = (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT);
 	const bool checkDynamic = !hil_enabled;
-	const bool checkPower = (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check);
 
 	bool checkAirspeed = false;
 
@@ -245,7 +243,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}
 
 	/* ---- RC CALIBRATION ---- */
-	if (checkRC) {
+	if (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT) {
 		if (rcCalibrationCheck(mavlink_log_pub, reportFailures, status.is_vtol) != OK) {
 			if (reportFailures) {
 				mavlink_log_critical(mavlink_log_pub, "RC calibration check failed");
@@ -265,7 +263,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 	}
 
 	/* ---- SYSTEM POWER ---- */
-	if (checkPower) {
+	if (status_flags.condition_power_input_valid && !status_flags.circuit_breaker_engaged_power_check) {
 		if (!powerCheck(mavlink_log_pub, status, reportFailures, prearm)) {
 			failed = true;
 		}

src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp

@@ -54,31 +54,37 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
 	system_power_sub.update();
 	const system_power_s &system_power = system_power_sub.get();
 
-	if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
+	if (hrt_elapsed_time(&system_power.timestamp) < 1_s) {
+		// Check avionics rail voltages (if USB isn't connected)
+		if (!system_power.usb_connected) {
+			float avionics_power_rail_voltage = system_power.voltage5v_v;
 
-		/* copy avionics voltage */
-		float avionics_power_rail_voltage = system_power.voltage5v_v;
+			if (avionics_power_rail_voltage < 4.5f) {
+				success = false;
 
-		// avionics rail
-		// Check avionics rail voltages
-		if (avionics_power_rail_voltage < 4.5f) {
-			success = false;
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
+							     (double)avionics_power_rail_voltage);
+				}
 
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Avionics Power low: %6.2f Volt",
-						     (double)avionics_power_rail_voltage);
-			}
+			} else if (avionics_power_rail_voltage < 4.9f) {
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
+				}
 
-		} else if (avionics_power_rail_voltage < 4.9f) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage);
-			}
-
-		} else if (avionics_power_rail_voltage > 5.4f) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
+			} else if (avionics_power_rail_voltage > 5.4f) {
+				if (report_fail) {
+					mavlink_log_critical(mavlink_log_pub, "CAUTION: Avionics Power high: %6.2f Volt", (double)avionics_power_rail_voltage);
+				}
 			}
 		}
+
+	} else {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "system power unavailable");
+		}
+
+		success = false;
 	}
 
 	return success;

src/modules/commander/Commander.cpp

@@ -1436,7 +1436,7 @@ Commander::run()
 			system_power_s system_power{};
 			_system_power_sub.copy(&system_power);
 
-			if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
+			if (hrt_elapsed_time(&system_power.timestamp) < 1_s) {
 				if (system_power.servo_valid &&
 				    !system_power.brick_valid &&
 				    !system_power.usb_connected) {
@@ -1449,24 +1449,28 @@ Commander::run()
 
 #if defined(CONFIG_BOARDCTL_RESET)
 
-				/* if the USB hardware connection went away, reboot */
-				if (status_flags.usb_connected && !system_power.usb_connected) {
-					/*
-					 * Apparently the USB cable went away but we are still powered,
-					 * so we bring the system back to a nominal state for flight.
-					 * This is important to unload the USB stack of the OS which is
-					 * a relatively complex piece of software that is non-essential
-					 * for flight and continuing to run it would add a software risk
-					 * without a need. The clean approach to unload it is to reboot.
-					 */
-					if (shutdown_if_allowed() && (px4_reboot_request(400_ms) == 0)) {
-						mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety");
+				if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) {
+					/* if the USB hardware connection went away, reboot */
+					if (_system_power_usb_connected && !system_power.usb_connected) {
+						/*
+						 * Apparently the USB cable went away but we are still powered,
+						 * so we bring the system back to a nominal state for flight.
+						 * This is important to unload the USB stack of the OS which is
+						 * a relatively complex piece of software that is non-essential
+						 * for flight and continuing to run it would add a software risk
+						 * without a need. The clean approach to unload it is to reboot.
+						 */
+						if (shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
+							mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting for flight safety");
 
-						while (1) { px4_usleep(1); }
+							while (1) { px4_usleep(1); }
+						}
 					}
 				}
 
 #endif // CONFIG_BOARDCTL_RESET
+
+				_system_power_usb_connected = system_power.usb_connected;
 			}
 		}
 

src/modules/commander/Commander.hpp

@@ -366,6 +366,7 @@ private:
 	bool		_have_taken_off_since_arming{false};
 	bool		_should_set_home_on_takeoff{true};
 	bool		_flight_termination_printed{false};
+	bool		_system_power_usb_connected{false};
 
 	main_state_t	_main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL};