diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index 5168ca4c26..c2ae24c3d5 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -34,34 +34,16 @@ GCS_MAVLINK::GCS_MAVLINK() :
 void
 GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
 {
+    // sanity check chan
+    if (mav_chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return;
+    }
+
     _port = port;
     chan = mav_chan;
 
-    switch (chan) {
-        case MAVLINK_COMM_0:
-            mavlink_comm_0_port = _port;
-            initialised = true;
-            break;
-        case MAVLINK_COMM_1:
-            mavlink_comm_1_port = _port;
-            initialised = true;
-            break;
-        case MAVLINK_COMM_2:
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-            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
-            break;
-    }
+    mavlink_comm_port[chan] = _port;
+    initialised = true;
     _queued_parameter = NULL;
     reset_cli_timeout();
 }
@@ -419,48 +401,17 @@ void GCS_MAVLINK::handle_gimbal_report(AP_Mount &mount, mavlink_message_t *msg)
  */
 bool GCS_MAVLINK::have_flow_control(void)
 {
-    switch (chan) {
-    case MAVLINK_COMM_0:
-        if (mavlink_comm_0_port == NULL) {
-            return false;
-        } else {
-            // assume USB has flow control
-            return hal.gpio->usb_connected() || mavlink_comm_0_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
-        }
-        break;
-
-    case MAVLINK_COMM_1:
-        if (mavlink_comm_1_port == NULL) {
-            return false;
-        } else {
-            return mavlink_comm_1_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
-        }
-        break;
-
-    case MAVLINK_COMM_2:
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-        if (mavlink_comm_2_port == NULL) {
-            return false;
-        } else {
-            return mavlink_comm_2_port != NULL && mavlink_comm_2_port->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
-        }
-        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;
+    // sanity check chan
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return false;
+    }
+
+    if (mavlink_comm_port[chan] == NULL) {
+        return false;
+    } else {
+        // assume USB has flow control
+        return hal.gpio->usb_connected() || mavlink_comm_port[chan]->get_flow_control() != AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE;
     }
-    return false;
 }
 
 
diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
index 8e876f24cc..69ed2f1af7 100644
--- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp
@@ -31,14 +31,7 @@ This provides some support code and variables for MAVLink enabled sketches
 #include "include/mavlink/v1.0/mavlink_helpers.h"
 #endif
 
-AP_HAL::UARTDriver	*mavlink_comm_0_port;
-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
+AP_HAL::UARTDriver	*mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
 
 mavlink_system_t mavlink_system = {7,1};
 
@@ -91,28 +84,12 @@ uint8_t mav_var_type(enum ap_var_type t)
 ///
 uint8_t comm_receive_ch(mavlink_channel_t chan)
 {
-    uint8_t data = 0;
-    switch(chan) {
-	case MAVLINK_COMM_0:
-		data = mavlink_comm_0_port->read();
-		break;
-	case MAVLINK_COMM_1:
-		data = mavlink_comm_1_port->read();
-		break;
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-	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;
-	}
-    return data;
+    // sanity check chan
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return 0;
+    }
+
+    return (uint8_t)mavlink_comm_port[chan]->read();
 }
 
 /// Check for available transmit space on the nominated MAVLink channel
@@ -121,30 +98,14 @@ uint8_t comm_receive_ch(mavlink_channel_t chan)
 /// @returns		Number of bytes available
 uint16_t comm_get_txspace(mavlink_channel_t chan)
 {
+    // sanity check chan
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return 0;
+    }
     if ((1U<<chan) & mavlink_locked_mask) {
         return 0;
     }
-	int16_t ret = 0;
-    switch(chan) {
-	case MAVLINK_COMM_0:
-		ret = mavlink_comm_0_port->txspace();
-		break;
-	case MAVLINK_COMM_1:
-		ret = mavlink_comm_1_port->txspace();
-		break;
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-	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;
-	}
+	int16_t ret = mavlink_comm_port[chan]->txspace();
 	if (ret < 0) {
 		ret = 0;
 	}
@@ -157,30 +118,14 @@ uint16_t comm_get_txspace(mavlink_channel_t chan)
 /// @returns		Number of bytes available
 uint16_t comm_get_available(mavlink_channel_t chan)
 {
+    // sanity check chan
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return 0;
+    }
     if ((1U<<chan) & mavlink_locked_mask) {
         return 0;
     }
-    int16_t bytes = 0;
-    switch(chan) {
-	case MAVLINK_COMM_0:
-		bytes = mavlink_comm_0_port->available();
-		break;
-	case MAVLINK_COMM_1:
-		bytes = mavlink_comm_1_port->available();
-		break;
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-	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;
-	}
+    int16_t bytes = mavlink_comm_port[chan]->available();
 	if (bytes == -1) {
 		return 0;
 	}
@@ -192,26 +137,11 @@ uint16_t comm_get_available(mavlink_channel_t chan)
  */
 void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
 {
-    switch(chan) {
-	case MAVLINK_COMM_0:
-		mavlink_comm_0_port->write(buf, len);
-		break;
-	case MAVLINK_COMM_1:
-		mavlink_comm_1_port->write(buf, len);
-		break;
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-	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;
-	}
+    // sanity check chan
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return;
+    }
+    mavlink_comm_port[chan]->write(buf, len);
 }
 
 static const uint8_t mavlink_message_crc_progmem[256] PROGMEM = MAVLINK_MESSAGE_CRCS;
diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h
index 7886e14020..1324a3c33d 100644
--- a/libraries/GCS_MAVLink/GCS_MAVLink.h
+++ b/libraries/GCS_MAVLink/GCS_MAVLink.h
@@ -51,19 +51,7 @@
 #include "include/mavlink/v1.0/mavlink_types.h"
 
 /// MAVLink stream used for uartA
-extern AP_HAL::UARTDriver	*mavlink_comm_0_port;
-
-/// MAVLink stream used for uartC
-extern AP_HAL::UARTDriver	*mavlink_comm_1_port;
-
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-/// MAVLink stream used for uartD
-extern AP_HAL::UARTDriver	*mavlink_comm_2_port;
-#endif
-
-#if MAVLINK_COMM_NUM_BUFFERS > 3
-extern AP_HAL::UARTDriver   *mavlink_comm_3_port;
-#endif
+extern AP_HAL::UARTDriver	*mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
 
 /// MAVLink system definition
 extern mavlink_system_t mavlink_system;
@@ -75,26 +63,11 @@ extern mavlink_system_t mavlink_system;
 ///
 static inline void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
 {
-    switch(chan) {
-	case MAVLINK_COMM_0:
-		mavlink_comm_0_port->write(ch);
-		break;
-	case MAVLINK_COMM_1:
-		mavlink_comm_1_port->write(ch);
-		break;
-#if MAVLINK_COMM_NUM_BUFFERS > 2
-	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;
-	}
+    // sanity check chan
+    if (chan >= MAVLINK_COMM_NUM_BUFFERS) {
+        return;
+    }
+    mavlink_comm_port[chan]->write(ch);
 }
 
 void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len);