diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.cpp b/libraries/GCS_MAVLink/GCS_MAVLink.cpp index 4bfb6b5f0a..40d8c9daa7 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink.cpp @@ -64,6 +64,89 @@ uint8_t mav_var_type(enum ap_var_type t) } +/// Read a byte from the nominated MAVLink channel +/// +/// @param chan Channel to receive on +/// @returns Byte read +/// +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 + default: + break; + } + return data; +} + +/// Check for available transmit space on the nominated MAVLink channel +/// +/// @param chan Channel to check +/// @returns Number of bytes available +uint16_t comm_get_txspace(mavlink_channel_t chan) +{ + 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 + default: + break; + } + if (ret < 0) { + ret = 0; + } + return (uint16_t)ret; +} + +/// Check for available data on the nominated MAVLink channel +/// +/// @param chan Channel to check +/// @returns Number of bytes available +uint16_t comm_get_available(mavlink_channel_t chan) +{ + 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 + default: + break; + } + if (bytes == -1) { + return 0; + } + return (uint16_t)bytes; +} + /* send a buffer out a MAVLink channel */ diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index 616f5ef3db..a6f8ddd964 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -88,84 +88,20 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len); /// @param chan Channel to receive on /// @returns Byte read /// -static inline 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 - default: - break; - } - return data; -} +uint8_t comm_receive_ch(mavlink_channel_t chan); /// Check for available data on the nominated MAVLink channel /// /// @param chan Channel to check /// @returns Number of bytes available -static inline uint16_t comm_get_available(mavlink_channel_t chan) -{ - 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 - default: - break; - } - if (bytes == -1) { - return 0; - } - return (uint16_t)bytes; -} +uint16_t comm_get_available(mavlink_channel_t chan); /// Check for available transmit space on the nominated MAVLink channel /// /// @param chan Channel to check /// @returns Number of bytes available -static inline uint16_t comm_get_txspace(mavlink_channel_t chan) -{ - 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 - default: - break; - } - if (ret < 0) { - ret = 0; - } - return (uint16_t)ret; -} +uint16_t comm_get_txspace(mavlink_channel_t chan); #ifdef HAVE_CRC_ACCUMULATE // use the AVR C library implementation. This is a bit over twice as