mirror of https://github.com/ArduPilot/ardupilot
Plane: use new usb_connected() function
This commit is contained in:
parent
a5b29f44d5
commit
4bd6b2f428
|
@ -310,10 +310,8 @@ static AP_Camera camera(&relay);
|
|||
// Global variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// APM2 only
|
||||
#if USB_MUX_PIN > 0
|
||||
// remember if USB is connected, so we can adjust baud rate
|
||||
static bool usb_connected;
|
||||
#endif
|
||||
|
||||
/* Radio values
|
||||
* Channel assignments
|
||||
|
|
|
@ -515,22 +515,13 @@ static bool telemetry_delayed(mavlink_channel_t chan)
|
|||
if (tnow > (uint32_t)g.telem_delay) {
|
||||
return false;
|
||||
}
|
||||
#if USB_MUX_PIN > 0
|
||||
if (chan == MAVLINK_COMM_0 && usb_connected) {
|
||||
// this is an APM2 with USB telemetry
|
||||
if (chan == MAVLINK_COMM_0 && hal.gpio->usb_connected()) {
|
||||
// this is USB telemetry, so won't be an Xbee
|
||||
return false;
|
||||
}
|
||||
// we're either on the 2nd UART, or no USB cable is connected
|
||||
// we need to delay telemetry
|
||||
// we need to delay telemetry by the TELEM_DELAY time
|
||||
return true;
|
||||
#else
|
||||
if (chan == MAVLINK_COMM_0) {
|
||||
// we're on the USB port
|
||||
return false;
|
||||
}
|
||||
// don't send telemetry yet
|
||||
return true;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -94,7 +94,6 @@
|
|||
# define C_LED_PIN 35
|
||||
# define LED_ON HIGH
|
||||
# define LED_OFF LOW
|
||||
# define USB_MUX_PIN -1
|
||||
# define BATTERY_VOLT_PIN 0 // Battery voltage on A0
|
||||
# define BATTERY_CURR_PIN 1 // Battery current on A1
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
|
||||
|
@ -106,11 +105,6 @@
|
|||
# define C_LED_PIN 25
|
||||
# define LED_ON LOW
|
||||
# define LED_OFF HIGH
|
||||
#if TELEMETRY_UART2 == ENABLED
|
||||
# define USB_MUX_PIN -1
|
||||
#else
|
||||
# define USB_MUX_PIN 23
|
||||
#endif
|
||||
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1
|
||||
# define BATTERY_CURR_PIN 2 // Battery current on A2
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
|
||||
|
@ -138,7 +132,6 @@
|
|||
# define C_LED_PIN 25
|
||||
# define LED_ON LOW
|
||||
# define LED_OFF HIGH
|
||||
# define USB_MUX_PIN -1
|
||||
# define BATTERY_VOLT_PIN -1
|
||||
# define BATTERY_CURR_PIN -1
|
||||
# define CONFIG_INS_TYPE CONFIG_INS_PX4
|
||||
|
@ -710,3 +703,7 @@
|
|||
# define SERIAL_BUFSIZE 512
|
||||
#endif
|
||||
|
||||
#ifndef SERIAL2_BUFSIZE
|
||||
# define SERIAL2_BUFSIZE 256
|
||||
#endif
|
||||
|
||||
|
|
|
@ -70,24 +70,6 @@ static void run_cli(AP_HAL::UARTDriver *port)
|
|||
|
||||
static void init_ardupilot()
|
||||
{
|
||||
#if USB_MUX_PIN > 0
|
||||
// on the APM2 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
|
||||
|
@ -128,18 +110,17 @@ static void init_ardupilot()
|
|||
// more than 5ms remaining in your call to hal.scheduler->delay
|
||||
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
|
||||
|
||||
#if USB_MUX_PIN > 0
|
||||
usb_connected = hal.gpio->usb_connected();
|
||||
if (!usb_connected) {
|
||||
// we are not connected via USB, re-init UART0 with right
|
||||
// baud rate
|
||||
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
|
||||
}
|
||||
#else
|
||||
|
||||
// we have a 2nd serial port for telemetry
|
||||
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
|
||||
128, SERIAL_BUFSIZE);
|
||||
128, SERIAL2_BUFSIZE);
|
||||
gcs3.init(hal.uartC);
|
||||
#endif
|
||||
|
||||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
|
||||
|
@ -207,9 +188,9 @@ static void init_ardupilot()
|
|||
|
||||
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
|
||||
cliSerial->println_P(msg);
|
||||
#if USB_MUX_PIN == 0
|
||||
hal.uartC->println_P(msg);
|
||||
#endif
|
||||
if (gcs3.initialised) {
|
||||
hal.uartC->println_P(msg);
|
||||
}
|
||||
|
||||
startup_ground();
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
|
@ -524,8 +505,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud)
|
|||
|
||||
static void check_usb_mux(void)
|
||||
{
|
||||
#if USB_MUX_PIN > 0
|
||||
bool usb_check = !digitalRead(USB_MUX_PIN);
|
||||
bool usb_check = hal.gpio->usb_connected();
|
||||
if (usb_check == usb_connected) {
|
||||
return;
|
||||
}
|
||||
|
@ -537,7 +517,6 @@ static void check_usb_mux(void)
|
|||
} else {
|
||||
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue