From f5b1c84e2d1ae360dfd9a1b53b49099f33db9d20 Mon Sep 17 00:00:00 2001 From: "tridge60@gmail.com" Date: Fri, 18 Mar 2011 01:34:13 +0000 Subject: [PATCH] mavlink: sync with upstream this syncs with 7e1c274437f09a8a03a5bef990e14b93c8fc8b07 from March 17 2011 git-svn-id: https://arducopter.googlecode.com/svn/trunk@1782 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/GCS_MAVLink/include/common/common.h | 1 + .../include/common/mavlink_msg_attitude_new.h | 268 ++++++++++++++++++ libraries/GCS_MAVLink/include/mavlink_types.h | 19 +- libraries/GCS_MAVLink/include/protocol.h | 147 +++++++--- 4 files changed, 395 insertions(+), 40 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index eafeec6217..d684bd6a17 100644 --- a/libraries/GCS_MAVLink/include/common/common.h +++ b/libraries/GCS_MAVLink/include/common/common.h @@ -102,6 +102,7 @@ enum MAV_DATA_STREAM #include "./mavlink_msg_raw_imu.h" #include "./mavlink_msg_raw_pressure.h" #include "./mavlink_msg_attitude.h" +#include "./mavlink_msg_attitude_new.h" #include "./mavlink_msg_local_position.h" #include "./mavlink_msg_global_position.h" #include "./mavlink_msg_gps_raw.h" diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h new file mode 100644 index 0000000000..cb55e3cd91 --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude_new.h @@ -0,0 +1,268 @@ +// MESSAGE ATTITUDE PACKING + +#define MAVLINK_MSG_ID_ATTITUDE_NEW 30 +#define MAVLINK_MSG_LENGTH_ATTITUDE_NEW 8+4+4+4+4+4+4 + +#ifndef MAVLINK_DEACTIVATE_STRUCTS + +typedef struct __mavlink_attitude_new_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + +} mavlink_attitude_new_t; + + + +/** + * @brief Pack a attitude_new message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_new_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; + + i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) + i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) + i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) + i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) + i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) + i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a attitude_new message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_new_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; + + i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) + i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) + i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) + i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) + i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) + i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a attitude_new struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param attitude_new C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_new_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_new_t* attitude_new) +{ + return mavlink_msg_attitude_new_pack(system_id, component_id, msg, attitude_new->usec, attitude_new->roll, attitude_new->pitch, attitude_new->yaw, attitude_new->rollspeed, attitude_new->pitchspeed, attitude_new->yawspeed); +} + +#endif + +/** + * @brief Send a attitude_new message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_new_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ + uint16_t checksum; + comm_send_ch(chan, MAVLINK_STX); + crc_init(&checksum); + mavlink_send_uart_uint8_t(chan, MAVLINK_MSG_LENGTH_ATTITUDE_NEW, &checksum); + mavlink_send_uart_uint8_t(chan, mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq, &checksum); + // Increase sequence + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; + mavlink_send_uart_uint8_t(chan, mavlink_system.sysid, &checksum); + mavlink_send_uart_uint8_t(chan, mavlink_system.compid, &checksum); + mavlink_send_uart_uint8_t(chan, MAVLINK_MSG_ID_ATTITUDE_NEW, &checksum); + mavlink_send_uart_uint64_t(chan, usec, &checksum); + mavlink_send_uart_float(chan, roll, &checksum); + mavlink_send_uart_float(chan, pitch, &checksum); + mavlink_send_uart_float(chan, yaw, &checksum); + mavlink_send_uart_float(chan, rollspeed, &checksum); + mavlink_send_uart_float(chan, pitchspeed, &checksum); + mavlink_send_uart_float(chan, yawspeed, &checksum); + // Checksum complete, send it + comm_send_ch(chan, (uint8_t)(checksum & 0xFF)); + comm_send_ch(chan, (uint8_t)(checksum >> 8)); +} + +#endif +// MESSAGE ATTITUDE UNPACKING + +/** + * @brief Get field usec from attitude_new message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_attitude_new_get_usec(const mavlink_message_t* msg) +{ + generic_64bit r; + r.b[7] = (msg->payload)[0]; + r.b[6] = (msg->payload)[1]; + r.b[5] = (msg->payload)[2]; + r.b[4] = (msg->payload)[3]; + r.b[3] = (msg->payload)[4]; + r.b[2] = (msg->payload)[5]; + r.b[1] = (msg->payload)[6]; + r.b[0] = (msg->payload)[7]; + return (uint64_t)r.ll; +} + +/** + * @brief Get field roll from attitude_new message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_attitude_new_get_roll(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitch from attitude_new message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_attitude_new_get_pitch(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yaw from attitude_new message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_attitude_new_get_yaw(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field rollspeed from attitude_new message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_new_get_rollspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field pitchspeed from attitude_new message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_new_get_pitchspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field yawspeed from attitude_new message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_new_get_yawspeed(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +#ifndef MAVLINK_DEACTIVATE_STRUCTS + +/** + * @brief Decode a attitude_new message into a struct + * + * @param msg The message to decode + * @param attitude_new C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_new_decode(const mavlink_message_t* msg, mavlink_attitude_new_t* attitude_new) +{ + attitude_new->usec = mavlink_msg_attitude_new_get_usec(msg); + attitude_new->roll = mavlink_msg_attitude_new_get_roll(msg); + attitude_new->pitch = mavlink_msg_attitude_new_get_pitch(msg); + attitude_new->yaw = mavlink_msg_attitude_new_get_yaw(msg); + attitude_new->rollspeed = mavlink_msg_attitude_new_get_rollspeed(msg); + attitude_new->pitchspeed = mavlink_msg_attitude_new_get_pitchspeed(msg); + attitude_new->yawspeed = mavlink_msg_attitude_new_get_yawspeed(msg); +} + +#endif diff --git a/libraries/GCS_MAVLink/include/mavlink_types.h b/libraries/GCS_MAVLink/include/mavlink_types.h index 609cb7071f..1c92af3762 100644 --- a/libraries/GCS_MAVLink/include/mavlink_types.h +++ b/libraries/GCS_MAVLink/include/mavlink_types.h @@ -179,10 +179,21 @@ typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_NB, - MAVLINK_COMM_NB_HIGH = 16 - } mavlink_channel_t; + MAVLINK_COMM_3 +} mavlink_channel_t; + +/* + * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number + * of buffers they will use. If more are used, then the result will be + * a stack overrun + */ +#ifndef MAVLINK_COMM_NUM_BUFFERS +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +# define MAVLINK_COMM_NUM_BUFFERS 16 +#else +# define MAVLINK_COMM_NUM_BUFFERS 4 +#endif +#endif typedef enum { MAVLINK_PARSE_STATE_UNINIT=0, diff --git a/libraries/GCS_MAVLink/include/protocol.h b/libraries/GCS_MAVLink/include/protocol.h index c6aace92e8..ed1f9546b4 100644 --- a/libraries/GCS_MAVLink/include/protocol.h +++ b/libraries/GCS_MAVLink/include/protocol.h @@ -34,12 +34,7 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; -#else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -#endif - + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; return &m_mavlink_status[chan]; } @@ -73,7 +68,7 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t msg->compid = component_id; // One sequence number per component msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -202,11 +197,8 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) */ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; -#else - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; -#endif + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; + // Initializes only once, values keep unchanged after first initialization mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); @@ -391,23 +383,13 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag /* static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; - #else - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; - static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; - #endif + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; + static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; + static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS]; // Set a packet start candidate index if sign is start sign if (c == MAVLINK_STX) @@ -417,13 +399,8 @@ static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_me // Parse normally, if a CRC mismatch occurs retry with the next packet index } -//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; -//#else -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; -//#endif +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; //// Initializes only once, values keep unchanged after first initialization // mavlink_parse_state_initialize(&m_mavlink_status[chan]); // @@ -575,6 +552,7 @@ typedef union __generic_64bit { uint8_t b[8]; int64_t ll; ///< Long long (64 bit) + double d; ///< IEEE-754 double precision floating point } generic_64bit; /** @@ -883,6 +861,103 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch) } */ +static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum) +{ + crc_accumulate(b, checksum); + comm_send_ch(chan, b); +} + +static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum) +{ + crc_accumulate(b, checksum); + comm_send_ch(chan, b); +} + +static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum) +{ + char s; + s = (b>>8)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b & 0xff); + comm_send_ch(chan, s); + crc_accumulate(s, checksum); +} + +static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum) +{ + mavlink_send_uart_uint16_t(chan, b, checksum); +} + +static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum) +{ + char s; + s = (b>>24)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>16)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>8)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b & 0xff); + comm_send_ch(chan, s); + crc_accumulate(s, checksum); +} + +static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum) +{ + mavlink_send_uart_uint32_t(chan, b, checksum); +} + +static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum) +{ + char s; + s = (b>>56)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>48)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>40)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>32)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>24)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>16)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b>>8)&0xff; + comm_send_ch(chan, s); + crc_accumulate(s, checksum); + s = (b & 0xff); + comm_send_ch(chan, s); + crc_accumulate(s, checksum); +} + +static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum) +{ + mavlink_send_uart_uint64_t(chan, b, checksum); +} + +static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum) +{ + generic_32bit g; + g.f = b; + mavlink_send_uart_uint32_t(chan, g.i, checksum); +} + +static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum) +{ + generic_64bit g; + g.d = b; + mavlink_send_uart_uint32_t(chan, g.ll, checksum); +} static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) {