Commander: Read USB status

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;
 			}
 		}