diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 19eb656ab1..5168ca4c26 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -51,6 +51,12 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan) mavlink_comm_2_port = _port; initialised = true; break; +#endif + case MAVLINK_COMM_3: +#if MAVLINK_COMM_NUM_BUFFERS > 3 + mavlink_comm_3_port = _port; + initialised = true; + break; #endif default: // do nothing for unsupport mavlink channels @@ -441,6 +447,16 @@ bool GCS_MAVLINK::have_flow_control(void) break; #endif + case MAVLINK_COMM_3: +#if MAVLINK_COMM_NUM_BUFFERS > 3 + if (mavlink_comm_3_port == NULL) { + return false; + } else { + return mavlink_comm_3_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE; + } + break; +#endif + default: break; } diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index bd4da649ef..8e876f24cc 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -36,6 +36,9 @@ AP_HAL::UARTDriver *mavlink_comm_1_port; #if MAVLINK_COMM_NUM_BUFFERS > 2 AP_HAL::UARTDriver *mavlink_comm_2_port; #endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 +AP_HAL::UARTDriver *mavlink_comm_3_port; +#endif mavlink_system_t mavlink_system = {7,1}; @@ -100,6 +103,11 @@ uint8_t comm_receive_ch(mavlink_channel_t chan) case MAVLINK_COMM_2: data = mavlink_comm_2_port->read(); break; +#endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 + case MAVLINK_COMM_3: + data = mavlink_comm_3_port->read(); + break; #endif default: break; @@ -128,6 +136,11 @@ uint16_t comm_get_txspace(mavlink_channel_t chan) case MAVLINK_COMM_2: ret = mavlink_comm_2_port->txspace(); break; +#endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 + case MAVLINK_COMM_3: + ret = mavlink_comm_3_port->txspace(); + break; #endif default: break; @@ -159,6 +172,11 @@ uint16_t comm_get_available(mavlink_channel_t chan) case MAVLINK_COMM_2: bytes = mavlink_comm_2_port->available(); break; +#endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 + case MAVLINK_COMM_3: + bytes = mavlink_comm_3_port->available(); + break; #endif default: break; @@ -185,6 +203,11 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len) case MAVLINK_COMM_2: mavlink_comm_2_port->write(buf, len); break; +#endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 + case MAVLINK_COMM_3: + mavlink_comm_3_port->write(buf, len); + break; #endif default: break; diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index 1008334b3b..7886e14020 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -26,8 +26,8 @@ // only two telemetry ports on APM1/APM2 #define MAVLINK_COMM_NUM_BUFFERS 2 #else -// allow three telemetry ports on other boards -#define MAVLINK_COMM_NUM_BUFFERS 3 +// allow four telemetry ports on other boards +#define MAVLINK_COMM_NUM_BUFFERS 4 #endif /* @@ -61,6 +61,10 @@ extern AP_HAL::UARTDriver *mavlink_comm_1_port; extern AP_HAL::UARTDriver *mavlink_comm_2_port; #endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 +extern AP_HAL::UARTDriver *mavlink_comm_3_port; +#endif + /// MAVLink system definition extern mavlink_system_t mavlink_system; @@ -82,6 +86,11 @@ static inline void comm_send_ch(mavlink_channel_t chan, uint8_t ch) case MAVLINK_COMM_2: mavlink_comm_2_port->write(ch); break; +#endif +#if MAVLINK_COMM_NUM_BUFFERS > 3 + case MAVLINK_COMM_3: + mavlink_comm_3_port->write(ch); + break; #endif default: break;