USB-MUX: enable USB MUX switching for ArduCopter

ArduCopter/ArduCopter.pde

@@ -559,6 +559,11 @@ static bool				new_radio_frame;
 
 AP_Relay relay;
 
+#if USB_MUX_PIN > 0
+static bool usb_connected;
+#endif
+
+
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
 ////////////////////////////////////////////////////////////////////////////////
@@ -918,6 +923,9 @@ static void slow_loop()
 				update_motor_leds();
 			#endif
 
+#if USB_MUX_PIN > 0
+            check_usb_mux();
+#endif
 			break;
 
 		default:

ArduCopter/GCS.h

@@ -40,7 +40,10 @@ public:
 	///
 	/// @param	port		The stream over which messages are exchanged.
 	///
-	void		init(FastSerial *port) { _port = port; }
+	void		init(FastSerial *port) {
+        _port = port;
+        initialised = true;
+    }
 
 	/// Update GCS state.
 	///
@@ -83,6 +86,9 @@ public:
     // send streams which match frequency range
     void data_stream_send(uint16_t freqMin, uint16_t freqMax);
 
+    // set to true if this GCS link is active
+    bool initialised;
+
 protected:
 	/// The stream we are communicating over
 	FastSerial			*_port;

ArduCopter/GCS_Mavlink.pde

@@ -1722,6 +1722,9 @@ static void mavlink_delay(unsigned long t)
             gcs_update();
         }
         delay(1);
+#if USB_MUX_PIN > 0
+        check_usb_mux();
+#endif
     } while (millis() - tstart < t);
 
 	in_mavlink_delay = false;
@@ -1733,8 +1736,10 @@ static void mavlink_delay(unsigned long t)
 static void gcs_send_message(enum ap_message id)
 {
     gcs0.send_message(id);
+    if (gcs3.initialised) {
     gcs3.send_message(id);
 }
+}
 
 /*
   send data streams in the given rate range on both links
@@ -1742,8 +1747,10 @@ static void gcs_send_message(enum ap_message id)
 static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
 {
     gcs0.data_stream_send(freqMin, freqMax);
+    if (gcs3.initialised) {
     gcs3.data_stream_send(freqMin, freqMax);
 }
+}
 
 /*
   look for incoming commands on the GCS links
@@ -1751,20 +1758,26 @@ static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
 static void gcs_update(void)
 {
 	gcs0.update();
+    if (gcs3.initialised) {
     gcs3.update();
 }
+}
 
 static void gcs_send_text(gcs_severity severity, const char *str)
 {
     gcs0.send_text(severity, str);
+    if (gcs3.initialised) {
     gcs3.send_text(severity, str);
 }
+}
 
 static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
 {
     gcs0.send_text(severity, str);
+    if (gcs3.initialised) {
     gcs3.send_text(severity, str);
 }
+}
 
 /*
   send a low priority formatted message to the GCS

ArduCopter/config.h

@@ -107,6 +107,7 @@
 # define LED_OFF          LOW
 # define SLIDE_SWITCH_PIN 40
 # define PUSHBUTTON_PIN   41
+# define USB_MUX_PIN      -1
 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_PURPLE
 # define A_LED_PIN        27
 # define B_LED_PIN        26
@@ -116,6 +117,7 @@
 # define SLIDE_SWITCH_PIN (-1)
 # define PUSHBUTTON_PIN   (-1)
 # define CLI_SLIDER_ENABLED DISABLED
+# define USB_MUX_PIN 23
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/system.pde

@@ -55,6 +55,24 @@ static void run_cli(void)
 
 static void init_ardupilot()
 {
+#if USB_MUX_PIN > 0
+    // on the purple board we have a mux thet switches UART0 between
+    // USB and the board header. If the right ArduPPM firmware is
+    // installed we can detect if USB is connected using the
+    // USB_MUX_PIN
+    pinMode(USB_MUX_PIN, INPUT);
+
+    usb_connected = !digitalRead(USB_MUX_PIN);
+    if (!usb_connected) {
+        // USB is not connected, this means UART0 may be a Xbee, with
+        // its darned bricking problem. We can't write to it for at
+        // least one second after powering up. Simplest solution for
+        // now is to delay for 1 second. Something more elegant may be
+        // added later
+        delay(1000);
+    }
+#endif
+
 	// Console serial port
 	//
 	// The console port buffers are defined to be sufficiently large to support
@@ -175,15 +193,20 @@ static void init_ardupilot()
 	    AP_Var::load_all();
 	}
 
-	// Telemetry port.
-	//
-	// Not used if telemetry is going to the console.
-	//
-	// XXX for unidirectional protocols, we could (should) minimize
-	// the receive buffer, and the transmit buffer could also be
-	// shrunk for protocols that don't send large messages.
-	//
+	// init the GCS
+    gcs0.init(&Serial);
+
+#if USB_MUX_PIN > 0
+    if (!usb_connected) {
+        // we are not connected via USB, re-init UART0 with right
+        // baud rate
+        Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+    }
+#else
+    // we have a 2nd serial port for telemetry
     Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+	gcs3.init(&Serial3);
+#endif
 
 	#ifdef RADIO_OVERRIDE_DEFAULTS
 	{
@@ -225,10 +248,6 @@ static void init_ardupilot()
 	g_gps->init();			// GPS Initialization
     g_gps->callback = mavlink_delay;
 
-	// init the GCS
-    gcs0.init(&Serial);
-    gcs3.init(&Serial3);
-
 	if(g.compass_enabled)
 		init_compass();
 
@@ -594,3 +613,21 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
     //Serial.println_P(PSTR("Invalid SERIAL3_BAUD"));
     return default_baud;
 }
+
+#if USB_MUX_PIN > 0
+static void check_usb_mux(void)
+{
+    bool usb_check = !digitalRead(USB_MUX_PIN);
+    if (usb_check == usb_connected) {
+        return;
+    }
+
+    // the user has switched to/from the telemetry port
+    usb_connected = usb_check;
+    if (usb_connected) {
+        Serial.begin(SERIAL0_BAUD, 128, 128);
+    } else {
+        Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128);
+    }
+}
+#endif