commander: do not reboot on USB disconnect when armed

Signed-off-by: Andrei Korigodski <[email protected]>

src/modules/commander/commander.cpp

@@ -219,6 +219,8 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
+bool shutdown_allowed();
+
 transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy);
 
 /**
@@ -558,6 +560,20 @@ void print_status()
 
 static orb_advert_t status_pub;
 
+bool shutdown_allowed()
+{
+	return TRANSITION_DENIED != arming_state_transition(&status,
+	                                                    battery,
+	                                                    safety,
+	                                                    vehicle_status_s::ARMING_STATE_REBOOT,
+	                                                    &armed,
+	                                                    false /* fRunPreArmChecks */,
+	                                                    &mavlink_log_pub,
+	                                                    &status_flags,
+	                                                    arm_requirements,
+	                                                    hrt_elapsed_time(&commander_boot_timestamp));
+}
+
 transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy)
 {
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@@ -1664,6 +1680,17 @@ Commander::run()
 				} else {
 					status_flags.condition_power_input_valid = true;
 				}
+
+				/* if the USB hardware connection went away, reboot */
+				if (status_flags.usb_connected && !system_power.usb_connected && shutdown_allowed()) {
+					/*
+					 * apparently the USB cable went away but we are still powered,
+					 * so lets reset to a classic non-usb state.
+					 */
+					mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting.")
+					usleep(400000);
+					px4_shutdown_request(true, false);
+				}
 			}
 		}