diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5fb1eaaecb6b..dc6d61c23805 100644 --- a/src/modules/commander/commander.cpp +++ b/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); + } } }