diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/version.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/version.h index ccb6f48dae..be6c3da48e 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri May 25 17:56:06 2012" +#define MAVLINK_BUILD_DATE "Mon Jun 4 13:31:56 2012" #define MAVLINK_WIRE_PROTOCOL_VERSION "0.9" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/common/version.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/common/version.h index 172a302714..a0ae030382 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v0.9/common/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri May 25 17:56:06 2012" +#define MAVLINK_BUILD_DATE "Mon Jun 4 13:31:56 2012" #define MAVLINK_WIRE_PROTOCOL_VERSION "0.9" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/mavlink_helpers.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/mavlink_helpers.h index 804490add2..e3a8a55ebd 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v0.9/mavlink_helpers.h +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/mavlink_helpers.h @@ -23,7 +23,7 @@ MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) */ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) { - + #if MAVLINK_EXTERNAL_RX_BUFFER // No m_mavlink_message array defined in function, // has to be defined externally @@ -133,6 +133,25 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint } #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief re-send a message over a uart channel + * this is more stack efficient than re-marshalling the message + */ +MAVLINK_HELPER void mavlink_uart_resend(mavlink_channel_t chan, const mavlink_message_t *msg) +{ + uint8_t ck[2]; + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + + MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); + _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); + _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); + _mavlink_send_uart(chan, (const char *)ck, 2); + MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); +} + + /** * @brief Pack a message to send it over a serial byte stream */ diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/protocol.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/protocol.h index 7b3e3c0bd2..6990799e05 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v0.9/protocol.h +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/protocol.h @@ -60,6 +60,7 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); #endif // MAVLINK_CRC_EXTRA MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg); +MAVLINK_HELPER void mavlink_uart_resend(mavlink_channel_t chan, const mavlink_message_t *msg); MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg); MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c); MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status); diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h index 1fa7e057d0..d2724383d4 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri May 25 17:56:06 2012" +#define MAVLINK_BUILD_DATE "Mon Jun 4 13:31:56 2012" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h index ac55cc1a71..f8706b6a4c 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h @@ -5,7 +5,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Fri May 25 17:56:06 2012" +#define MAVLINK_BUILD_DATE "Mon Jun 4 13:31:56 2012" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_helpers.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_helpers.h index 39d6930e5b..6c7748a4aa 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_helpers.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_helpers.h @@ -133,6 +133,25 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint } #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief re-send a message over a uart channel + * this is more stack efficient than re-marshalling the message + */ +MAVLINK_HELPER void mav_uart_resend(mavlink_channel_t chan, const mavlink_message_t *msg) +{ + uint8_t ck[2]; + + ck[0] = (uint8_t)(msg->checksum & 0xFF); + ck[1] = (uint8_t)(msg->checksum >> 8); + + MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); + _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); + _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); + _mavlink_send_uart(chan, ck, 2); + MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); +} + + /** * @brief Pack a message to send it over a serial byte stream */