Plane: fixed uartA baud rates for APM1

This commit is contained in:
Andrew Tridgell 2013-09-21 09:16:17 +10:00
parent 7e819a52a7
commit b28a4f21f2
2 changed files with 12 additions and 7 deletions

View File

@ -710,7 +710,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ gcs_data_stream_send, 1, 3000 },
{ update_mount, 1, 1500 },
{ update_events, 15, 1500 },
{ check_usb_mux, 5, 1000 },
{ check_usb_mux, 5, 200 },
{ read_battery, 5, 1000 },
{ compass_accumulate, 1, 1500 },
{ barometer_accumulate, 1, 900 }, // 20

View File

@ -110,12 +110,10 @@ static void init_ardupilot()
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
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));
}
// we start by assuming USB connected, as we initialed the serial
// port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.
usb_connected = true;
check_usb_mux();
// we have a 2nd serial port for telemetry
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
@ -503,11 +501,18 @@ static void check_usb_mux(void)
// the user has switched to/from the telemetry port
usb_connected = usb_check;
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
// the APM2 has a MUX setup where the first serial port switches
// between USB and a TTL serial connection. When on USB we use
// SERIAL0_BAUD, but when connected as a TTL serial port we run it
// at SERIAL3_BAUD.
if (usb_connected) {
hal.uartA->begin(SERIAL0_BAUD);
} else {
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
#endif
}