Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
mavlink_msg_heartbeat.h
00001 // MESSAGE HEARTBEAT PACKING 00002 00003 #define MAVLINK_MSG_ID_HEARTBEAT 0 00004 00005 typedef struct __mavlink_heartbeat_t 00006 { 00007 uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00008 uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00009 uint8_t mavlink_version; ///< MAVLink version 00010 00011 } mavlink_heartbeat_t; 00012 00013 00014 00015 /** 00016 * @brief Pack a heartbeat message 00017 * @param system_id ID of this system 00018 * @param component_id ID of this component (e.g. 200 for IMU) 00019 * @param msg The MAVLink message to compress the data into 00020 * 00021 * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00022 * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00023 * @return length of the message in bytes (excluding serial stream start sign) 00024 */ 00025 static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) 00026 { 00027 uint16_t i = 0; 00028 msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; 00029 00030 i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00031 i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00032 i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version 00033 00034 return mavlink_finalize_message(msg, system_id, component_id, i); 00035 } 00036 00037 /** 00038 * @brief Pack a heartbeat message 00039 * @param system_id ID of this system 00040 * @param component_id ID of this component (e.g. 200 for IMU) 00041 * @param chan The MAVLink channel this message was sent over 00042 * @param msg The MAVLink message to compress the data into 00043 * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00044 * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00045 * @return length of the message in bytes (excluding serial stream start sign) 00046 */ 00047 static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t autopilot) 00048 { 00049 uint16_t i = 0; 00050 msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; 00051 00052 i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00053 i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00054 i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version 00055 00056 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); 00057 } 00058 00059 /** 00060 * @brief Encode a heartbeat struct into a message 00061 * 00062 * @param system_id ID of this system 00063 * @param component_id ID of this component (e.g. 200 for IMU) 00064 * @param msg The MAVLink message to compress the data into 00065 * @param heartbeat C-struct to read the message contents from 00066 */ 00067 static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) 00068 { 00069 return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot); 00070 } 00071 00072 /** 00073 * @brief Send a heartbeat message 00074 * @param chan MAVLink channel to send the message 00075 * 00076 * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00077 * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00078 */ 00079 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 00080 00081 static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) 00082 { 00083 mavlink_message_t msg; 00084 mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); 00085 mavlink_send_uart(chan, &msg); 00086 } 00087 00088 #endif 00089 // MESSAGE HEARTBEAT UNPACKING 00090 00091 /** 00092 * @brief Get field type from heartbeat message 00093 * 00094 * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) 00095 */ 00096 static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) 00097 { 00098 return (uint8_t)(msg->payload)[0]; 00099 } 00100 00101 /** 00102 * @brief Get field autopilot from heartbeat message 00103 * 00104 * @return Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM 00105 */ 00106 static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) 00107 { 00108 return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; 00109 } 00110 00111 /** 00112 * @brief Get field mavlink_version from heartbeat message 00113 * 00114 * @return MAVLink version 00115 */ 00116 static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) 00117 { 00118 return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; 00119 } 00120 00121 /** 00122 * @brief Decode a heartbeat message into a struct 00123 * 00124 * @param msg The message to decode 00125 * @param heartbeat C-struct to decode the message contents into 00126 */ 00127 static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) 00128 { 00129 heartbeat->type = mavlink_msg_heartbeat_get_type(msg); 00130 heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); 00131 heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); 00132 }
Generated on Tue Jul 12 2022 14:09:26 by 1.7.2