Browse Source

Plane: added SERIAL2_BAUD and rename SERIAL3_BAUD to SERIAL1_BAUD

mission-4.1.18
Andrew Tridgell 11 years ago
parent
commit
8383abc1fe
  1. 2
      ArduPlane/GCS_Mavlink.pde
  2. 8
      ArduPlane/Parameters.h
  3. 17
      ArduPlane/Parameters.pde
  4. 11
      ArduPlane/config.h
  5. 14
      ArduPlane/system.pde

2
ArduPlane/GCS_Mavlink.pde

@ -2184,7 +2184,7 @@ GCS_MAVLINK::queued_param_send() @@ -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);
}

8
ArduPlane/Parameters.h

@ -105,10 +105,11 @@ public: @@ -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: @@ -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

17
ArduPlane/Parameters.pde

@ -31,17 +31,26 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -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

11
ArduPlane/config.h

@ -164,8 +164,11 @@ @@ -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 @@ @@ -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

14
ArduPlane/system.pde

@ -116,15 +116,17 @@ static void init_ardupilot() @@ -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) @@ -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) @@ -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
}

Loading…
Cancel
Save