From 8383abc1feac18d3ce6987649932132c4775a25e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Nov 2013 10:58:11 +1100 Subject: [PATCH] Plane: added SERIAL2_BAUD and rename SERIAL3_BAUD to SERIAL1_BAUD --- ArduPlane/GCS_Mavlink.pde | 2 +- ArduPlane/Parameters.h | 8 ++++++-- ArduPlane/Parameters.pde | 17 +++++++++++++---- ArduPlane/config.h | 11 +++++++++-- ArduPlane/system.pde | 14 ++++++++------ 5 files changed, 37 insertions(+), 15 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index fa268e3148..d861294e8c 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -2184,7 +2184,7 @@ GCS_MAVLINK::queued_param_send() // use at most 30% of bandwidth on parameters. The constant 26 is // 1/(1000 * 1/8 * 0.001 * 0.3) - bytes_allowed = g.serial3_baud * (tnow - _queued_parameter_send_time_ms) * 26; + bytes_allowed = g.serial1_baud * (tnow - _queued_parameter_send_time_ms) * 26; if (bytes_allowed > comm_get_txspace(chan)) { bytes_allowed = comm_get_txspace(chan); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index c795e49cb5..ecf60eb765 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -105,10 +105,11 @@ public: k_param_gcs1, // stream rates for uartC k_param_sysid_this_mav, k_param_sysid_my_gcs, - k_param_serial3_baud, + k_param_serial1_baud, k_param_telem_delay, k_param_serial0_baud, k_param_gcs2, // stream rates for uartD + k_param_serial2_baud, // 120: Fly-by-wire control // @@ -272,7 +273,10 @@ public: AP_Int16 sysid_this_mav; AP_Int16 sysid_my_gcs; AP_Int8 serial0_baud; - AP_Int8 serial3_baud; + AP_Int8 serial1_baud; +#if MAVLINK_COMM_NUM_BUFFERS > 2 + AP_Int8 serial2_baud; +#endif AP_Int8 telem_delay; #if HIL_MODE != HIL_MODE_DISABLED diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 3dcea3498b..39db5768d3 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -31,17 +31,26 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: SERIAL0_BAUD // @DisplayName: USB Console Baud Rate - // @Description: The baud rate used on the main uart + // @Description: The baud rate used on the USB console // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @User: Standard GSCALAR(serial0_baud, "SERIAL0_BAUD", SERIAL0_BAUD/1000), - // @Param: SERIAL3_BAUD + // @Param: SERIAL1_BAUD // @DisplayName: Telemetry Baud Rate - // @Description: The baud rate used on the telemetry port + // @Description: The baud rate used on the first telemetry port // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 // @User: Standard - GSCALAR(serial3_baud, "SERIAL3_BAUD", SERIAL3_BAUD/1000), + GSCALAR(serial1_baud, "SERIAL1_BAUD", SERIAL1_BAUD/1000), + +#if MAVLINK_COMM_NUM_BUFFERS > 2 + // @Param: SERIAL2_BAUD + // @DisplayName: Telemetry Baud Rate + // @Description: The baud rate used on the second telemetry port + // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200 + // @User: Standard + GSCALAR(serial2_baud, "SERIAL2_BAUD", SERIAL2_BAUD/1000), +#endif // @Param: TELEM_DELAY // @DisplayName: Telemetry startup delay diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 0c950a1927..20a026ef3b 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -164,8 +164,11 @@ #ifndef SERIAL0_BAUD # define SERIAL0_BAUD 115200 #endif -#ifndef SERIAL3_BAUD - # define SERIAL3_BAUD 57600 +#ifndef SERIAL1_BAUD + # define SERIAL1_BAUD 57600 +#endif +#ifndef SERIAL2_BAUD + # define SERIAL2_BAUD 57600 #endif @@ -526,6 +529,10 @@ # define SERIAL_BUFSIZE 512 #endif +#ifndef SERIAL1_BUFSIZE + # define SERIAL1_BUFSIZE 256 +#endif + #ifndef SERIAL2_BUFSIZE # define SERIAL2_BUFSIZE 256 #endif diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 21b3e28a30..5bf3bf6262 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -116,15 +116,17 @@ static void init_ardupilot() check_usb_mux(); // we have a 2nd serial port for telemetry - hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), - 128, SERIAL2_BUFSIZE); + hal.uartC->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD), + 128, SERIAL1_BUFSIZE); gcs[1].init(hal.uartC); +#if MAVLINK_COMM_NUM_BUFFERS > 2 if (hal.uartD != NULL) { - hal.uartD->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), + hal.uartD->begin(map_baudrate(g.serial2_baud, SERIAL2_BAUD), 128, SERIAL2_BUFSIZE); gcs[2].init(hal.uartD); } +#endif mavlink_system.sysid = g.sysid_this_mav; @@ -494,7 +496,7 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) case 111: return 111100; case 115: return 115200; } - cliSerial->println_P(PSTR("Invalid SERIAL3_BAUD")); + cliSerial->println_P(PSTR("Invalid baudrate")); return default_baud; } @@ -513,11 +515,11 @@ static void check_usb_mux(void) // 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. + // at SERIAL1_BAUD. if (usb_connected) { hal.uartA->begin(SERIAL0_BAUD); } else { - hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD)); + hal.uartA->begin(map_baudrate(g.serial1_baud, SERIAL1_BAUD)); } #endif }