diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 96e305f8b9..931e3e54b1 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -235,6 +235,10 @@ byte control_mode = INITIALISING; byte oldSwitchPosition; // for remembering the control mode switch bool inverted_flight = false; +#if USB_MUX_PIN > 0 +static bool usb_connected; +#endif + static const char *comma = ","; static const char* flight_mode_strings[] = { @@ -738,6 +742,11 @@ static void slow_loop() mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter gcs_data_stream_send(3,5); + +#if USB_MUX_PIN > 0 + check_usb_mux(); +#endif + break; } } diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 7172eeaf52..dfee2c77cb 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/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; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index e30ea70b75..2203c2920f 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -592,7 +592,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, CHECK_PAYLOAD_SIZE(PARAM_VALUE); if (chan == MAVLINK_COMM_0) { gcs0.queued_param_send(); - } else { + } else if (gcs3.initialised) { gcs3.queued_param_send(); } break; @@ -601,7 +601,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); if (chan == MAVLINK_COMM_0) { gcs0.queued_waypoint_send(); - } else { + } else if (gcs3.initialised) { gcs3.queued_waypoint_send(); } break; @@ -2022,6 +2022,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; @@ -2033,7 +2036,9 @@ static void mavlink_delay(unsigned long t) static void gcs_send_message(enum ap_message id) { gcs0.send_message(id); - gcs3.send_message(id); + if (gcs3.initialised) { + gcs3.send_message(id); + } } /* @@ -2042,7 +2047,9 @@ 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); - gcs3.data_stream_send(freqMin, freqMax); + if (gcs3.initialised) { + gcs3.data_stream_send(freqMin, freqMax); + } } /* @@ -2051,19 +2058,25 @@ static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax) static void gcs_update(void) { gcs0.update(); - gcs3.update(); + if (gcs3.initialised) { + gcs3.update(); + } } static void gcs_send_text(gcs_severity severity, const char *str) { gcs0.send_text(severity, str); - gcs3.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); - gcs3.send_text(severity, str); + if (gcs3.initialised) { + gcs3.send_text(severity, str); + } } /* diff --git a/ArduPlane/config.h b/ArduPlane/config.h index a94248956e..6342652de8 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -72,6 +72,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 @@ -81,6 +82,7 @@ # define SLIDE_SWITCH_PIN (-1) # define PUSHBUTTON_PIN (-1) # define CLI_SLIDER_ENABLED DISABLED +# define USB_MUX_PIN 23 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde index a79f5d7f3a..94ef061e46 100644 --- a/ArduPlane/planner.pde +++ b/ArduPlane/planner.pde @@ -26,8 +26,12 @@ static int8_t planner_gcs(uint8_t argc, const Menu::arg *argv) { gcs0.init(&Serial); + +#if USB_MUX_PIN > 0 + // we don't have gcs3 if we have the USB mux setup gcs3.init(&Serial3); - +#endif + int loopcount = 0; while (1) { if (millis()-fast_loopTimer > 19) { diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 19370bc74e..964c075d79 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/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 @@ -136,15 +154,20 @@ static void init_ardupilot() // used to detect in-flight resets g.num_resets.set_and_save(g.num_resets+1); - // 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. - // - Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, 128); + // 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 mavlink_system.sysid = g.sysid_this_mav; @@ -180,10 +203,6 @@ static void init_ardupilot() g_gps->init(); // GPS Initialization g_gps->callback = mavlink_delay; - // init the GCS - gcs0.init(&Serial); - gcs3.init(&Serial3); - //mavlink_system.sysid = MAV_SYSTEM_ID; // Using g.sysid_this_mav mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id mavlink_system.type = MAV_FIXED_WING; @@ -518,3 +537,22 @@ 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