diff options
Diffstat (limited to 'mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h')
-rw-r--r-- | mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h | 61 |
1 files changed, 47 insertions, 14 deletions
diff --git a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h index 599ea0bc5..826138fad 100644 --- a/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h +++ b/mavlink/include/mavlink/v1.0/common/mavlink_msg_heartbeat.h @@ -15,6 +15,9 @@ typedef struct __mavlink_heartbeat_t #define MAVLINK_MSG_ID_HEARTBEAT_LEN 9 #define MAVLINK_MSG_ID_0_LEN 9 +#define MAVLINK_MSG_ID_HEARTBEAT_CRC 50 +#define MAVLINK_MSG_ID_0_CRC 50 + #define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ @@ -47,7 +50,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[9]; + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; _mav_put_uint32_t(buf, 0, custom_mode); _mav_put_uint8_t(buf, 4, type); _mav_put_uint8_t(buf, 5, autopilot); @@ -55,7 +58,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com _mav_put_uint8_t(buf, 7, system_status); _mav_put_uint8_t(buf, 8, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); #else mavlink_heartbeat_t packet; packet.custom_mode = custom_mode; @@ -65,18 +68,22 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com packet.system_status = system_status; packet.mavlink_version = 3; - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); #endif msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - return mavlink_finalize_message(msg, system_id, component_id, 9, 50); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif } /** * @brief Pack a heartbeat message on a channel * @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 chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM @@ -90,7 +97,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ uint8_t type,uint8_t autopilot,uint8_t base_mode,uint32_t custom_mode,uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[9]; + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; _mav_put_uint32_t(buf, 0, custom_mode); _mav_put_uint8_t(buf, 4, type); _mav_put_uint8_t(buf, 5, autopilot); @@ -98,7 +105,7 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 7, system_status); _mav_put_uint8_t(buf, 8, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); #else mavlink_heartbeat_t packet; packet.custom_mode = custom_mode; @@ -108,15 +115,19 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ packet.system_status = system_status; packet.mavlink_version = 3; - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 9); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); #endif msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 50); +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif } /** - * @brief Encode a heartbeat struct into a message + * @brief Encode a heartbeat struct * * @param system_id ID of this system * @param component_id ID of this component (e.g. 200 for IMU) @@ -129,6 +140,20 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c } /** + * @brief Encode a heartbeat struct on a channel + * + * @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 will be sent over + * @param msg The MAVLink message to compress the data into + * @param heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack_chan(system_id, component_id, chan, msg, heartbeat->type, heartbeat->autopilot, heartbeat->base_mode, heartbeat->custom_mode, heartbeat->system_status); +} + +/** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message * @@ -143,7 +168,7 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint32_t custom_mode, uint8_t system_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[9]; + char buf[MAVLINK_MSG_ID_HEARTBEAT_LEN]; _mav_put_uint32_t(buf, 0, custom_mode); _mav_put_uint8_t(buf, 4, type); _mav_put_uint8_t(buf, 5, autopilot); @@ -151,7 +176,11 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty _mav_put_uint8_t(buf, 7, system_status); _mav_put_uint8_t(buf, 8, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, 9, 50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif #else mavlink_heartbeat_t packet; packet.custom_mode = custom_mode; @@ -161,7 +190,11 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty packet.system_status = system_status; packet.mavlink_version = 3; - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, 9, 50); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN, MAVLINK_MSG_ID_HEARTBEAT_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, MAVLINK_MSG_ID_HEARTBEAT_LEN); +#endif #endif } @@ -246,6 +279,6 @@ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, ma heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); #else - memcpy(heartbeat, _MAV_PAYLOAD(msg), 9); + memcpy(heartbeat, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HEARTBEAT_LEN); #endif } |