diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67ce676df5..3573f5d6eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -180,6 +180,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static bool _usb_telemetry_active = false; static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; @@ -200,7 +201,7 @@ static struct home_position_s _home; static unsigned _last_mission_instance = 0; static manual_control_setpoint_s _last_sp_man; -struct vtol_vehicle_status_s vtol_status; +static struct vtol_vehicle_status_s vtol_status; /** * The daemon app only briefly exists to start @@ -1463,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[]) } } + /* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */ + if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) { + _usb_telemetry_active = true; + } + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; } } @@ -1519,18 +1525,19 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + /* if the USB hardware connection went away, reboot */ if (status.usb_connected && !system_power.usb_connected) { /* * apparently the USB cable went away but we are still powered, * so lets reset to a classic non-usb state. */ - usleep(100000); mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") usleep(400000); px4_systemreset(false); } - status.usb_connected = system_power.usb_connected; + /* finally judge the USB connected state based on software detection */ + status.usb_connected = _usb_telemetry_active; } }