[88] | 1 | #ifndef MAVLINK_TYPES_H_
|
---|
| 2 | #define MAVLINK_TYPES_H_
|
---|
| 3 |
|
---|
| 4 | // Visual Studio versions before 2010 don't have stdint.h, so we just error out.
|
---|
| 5 | #if (defined _MSC_VER) && (_MSC_VER < 1600)
|
---|
| 6 | #error "The C-MAVLink implementation requires Visual Studio 2010 or greater"
|
---|
| 7 | #endif
|
---|
| 8 |
|
---|
| 9 | #include <stdint.h>
|
---|
| 10 |
|
---|
| 11 | // Macro to define packed structures
|
---|
| 12 | #ifdef __GNUC__
|
---|
| 13 | #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed))
|
---|
| 14 | #else
|
---|
| 15 | #define MAVPACKED( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) )
|
---|
| 16 | #endif
|
---|
| 17 |
|
---|
| 18 | #ifndef MAVLINK_MAX_PAYLOAD_LEN
|
---|
| 19 | // it is possible to override this, but be careful!
|
---|
| 20 | #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
|
---|
| 21 | #endif
|
---|
| 22 |
|
---|
| 23 | #define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
|
---|
| 24 | #define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
|
---|
| 25 | #define MAVLINK_NUM_CHECKSUM_BYTES 2
|
---|
| 26 | #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
|
---|
| 27 |
|
---|
| 28 | #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
|
---|
| 29 |
|
---|
| 30 | #define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
|
---|
| 31 | #define MAVLINK_EXTENDED_HEADER_LEN 14
|
---|
| 32 |
|
---|
| 33 | #if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__)
|
---|
| 34 | /* full fledged 32bit++ OS */
|
---|
| 35 | #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
|
---|
| 36 | #else
|
---|
| 37 | /* small microcontrollers */
|
---|
| 38 | #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
|
---|
| 39 | #endif
|
---|
| 40 |
|
---|
| 41 | #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
|
---|
| 42 |
|
---|
| 43 |
|
---|
| 44 | /**
|
---|
| 45 | * Old-style 4 byte param union
|
---|
| 46 | *
|
---|
| 47 | * This struct is the data format to be used when sending
|
---|
| 48 | * parameters. The parameter should be copied to the native
|
---|
| 49 | * type (without type conversion)
|
---|
| 50 | * and re-instanted on the receiving side using the
|
---|
| 51 | * native type as well.
|
---|
| 52 | */
|
---|
| 53 | MAVPACKED(
|
---|
| 54 | typedef struct param_union {
|
---|
| 55 | union {
|
---|
| 56 | float param_float;
|
---|
| 57 | int32_t param_int32;
|
---|
| 58 | uint32_t param_uint32;
|
---|
| 59 | int16_t param_int16;
|
---|
| 60 | uint16_t param_uint16;
|
---|
| 61 | int8_t param_int8;
|
---|
| 62 | uint8_t param_uint8;
|
---|
| 63 | uint8_t bytes[4];
|
---|
| 64 | };
|
---|
| 65 | uint8_t type;
|
---|
| 66 | }) mavlink_param_union_t;
|
---|
| 67 |
|
---|
| 68 |
|
---|
| 69 | /**
|
---|
| 70 | * New-style 8 byte param union
|
---|
| 71 | * mavlink_param_union_double_t will be 8 bytes long, and treated as needing 8 byte alignment for the purposes of MAVLink 1.0 field ordering.
|
---|
| 72 | * The mavlink_param_union_double_t will be treated as a little-endian structure.
|
---|
| 73 | *
|
---|
| 74 | * If is_double is 1 then the type is a double, and the remaining 63 bits are the double, with the lowest bit of the mantissa zero.
|
---|
| 75 | * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the
|
---|
| 76 | * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union.
|
---|
| 77 | * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above,
|
---|
| 78 | * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86,
|
---|
| 79 | * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number,
|
---|
| 80 | * and the bits pulled out using the shifts/masks.
|
---|
| 81 | */
|
---|
| 82 | MAVPACKED(
|
---|
| 83 | typedef struct param_union_extended {
|
---|
| 84 | union {
|
---|
| 85 | struct {
|
---|
| 86 | uint8_t is_double:1;
|
---|
| 87 | uint8_t mavlink_type:7;
|
---|
| 88 | union {
|
---|
| 89 | char c;
|
---|
| 90 | uint8_t uint8;
|
---|
| 91 | int8_t int8;
|
---|
| 92 | uint16_t uint16;
|
---|
| 93 | int16_t int16;
|
---|
| 94 | uint32_t uint32;
|
---|
| 95 | int32_t int32;
|
---|
| 96 | float f;
|
---|
| 97 | uint8_t align[7];
|
---|
| 98 | };
|
---|
| 99 | };
|
---|
| 100 | uint8_t data[8];
|
---|
| 101 | };
|
---|
| 102 | }) mavlink_param_union_double_t;
|
---|
| 103 |
|
---|
| 104 | /**
|
---|
| 105 | * This structure is required to make the mavlink_send_xxx convenience functions
|
---|
| 106 | * work, as it tells the library what the current system and component ID are.
|
---|
| 107 | */
|
---|
| 108 | MAVPACKED(
|
---|
| 109 | typedef struct __mavlink_system {
|
---|
| 110 | uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function
|
---|
| 111 | uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function
|
---|
| 112 | }) mavlink_system_t;
|
---|
| 113 |
|
---|
| 114 | MAVPACKED(
|
---|
| 115 | typedef struct __mavlink_message {
|
---|
| 116 | uint16_t checksum; ///< sent at end of packet
|
---|
| 117 | uint8_t magic; ///< protocol magic marker
|
---|
| 118 | uint8_t len; ///< Length of payload
|
---|
| 119 | uint8_t seq; ///< Sequence of packet
|
---|
| 120 | uint8_t sysid; ///< ID of message sender system/aircraft
|
---|
| 121 | uint8_t compid; ///< ID of the message sender component
|
---|
| 122 | uint8_t msgid; ///< ID of message in payload
|
---|
| 123 | uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
|
---|
| 124 | }) mavlink_message_t;
|
---|
| 125 |
|
---|
| 126 | MAVPACKED(
|
---|
| 127 | typedef struct __mavlink_extended_message {
|
---|
| 128 | mavlink_message_t base_msg;
|
---|
| 129 | int32_t extended_payload_len; ///< Length of extended payload if any
|
---|
| 130 | uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
|
---|
| 131 | }) mavlink_extended_message_t;
|
---|
| 132 |
|
---|
| 133 | typedef enum {
|
---|
| 134 | MAVLINK_TYPE_CHAR = 0,
|
---|
| 135 | MAVLINK_TYPE_UINT8_T = 1,
|
---|
| 136 | MAVLINK_TYPE_INT8_T = 2,
|
---|
| 137 | MAVLINK_TYPE_UINT16_T = 3,
|
---|
| 138 | MAVLINK_TYPE_INT16_T = 4,
|
---|
| 139 | MAVLINK_TYPE_UINT32_T = 5,
|
---|
| 140 | MAVLINK_TYPE_INT32_T = 6,
|
---|
| 141 | MAVLINK_TYPE_UINT64_T = 7,
|
---|
| 142 | MAVLINK_TYPE_INT64_T = 8,
|
---|
| 143 | MAVLINK_TYPE_FLOAT = 9,
|
---|
| 144 | MAVLINK_TYPE_DOUBLE = 10
|
---|
| 145 | } mavlink_message_type_t;
|
---|
| 146 |
|
---|
| 147 | #define MAVLINK_MAX_FIELDS 64
|
---|
| 148 |
|
---|
| 149 | typedef struct __mavlink_field_info {
|
---|
| 150 | const char *name; // name of this field
|
---|
| 151 | const char *print_format; // printing format hint, or NULL
|
---|
| 152 | mavlink_message_type_t type; // type of this field
|
---|
| 153 | unsigned int array_length; // if non-zero, field is an array
|
---|
| 154 | unsigned int wire_offset; // offset of each field in the payload
|
---|
| 155 | unsigned int structure_offset; // offset in a C structure
|
---|
| 156 | } mavlink_field_info_t;
|
---|
| 157 |
|
---|
| 158 | // note that in this structure the order of fields is the order
|
---|
| 159 | // in the XML file, not necessary the wire order
|
---|
| 160 | typedef struct __mavlink_message_info {
|
---|
| 161 | const char *name; // name of the message
|
---|
| 162 | unsigned num_fields; // how many fields in this message
|
---|
| 163 | mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information
|
---|
| 164 | } mavlink_message_info_t;
|
---|
| 165 |
|
---|
| 166 | #define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0])))
|
---|
| 167 | #define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
|
---|
| 168 |
|
---|
| 169 | // checksum is immediately after the payload bytes
|
---|
| 170 | #define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
|
---|
| 171 | #define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
|
---|
| 172 |
|
---|
| 173 | typedef enum {
|
---|
| 174 | MAVLINK_COMM_0,
|
---|
| 175 | MAVLINK_COMM_1,
|
---|
| 176 | MAVLINK_COMM_2,
|
---|
| 177 | MAVLINK_COMM_3
|
---|
| 178 | } mavlink_channel_t;
|
---|
| 179 |
|
---|
| 180 | /*
|
---|
| 181 | * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
|
---|
| 182 | * of buffers they will use. If more are used, then the result will be
|
---|
| 183 | * a stack overrun
|
---|
| 184 | */
|
---|
| 185 | #ifndef MAVLINK_COMM_NUM_BUFFERS
|
---|
| 186 | #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
|
---|
| 187 | # define MAVLINK_COMM_NUM_BUFFERS 16
|
---|
| 188 | #else
|
---|
| 189 | # define MAVLINK_COMM_NUM_BUFFERS 4
|
---|
| 190 | #endif
|
---|
| 191 | #endif
|
---|
| 192 |
|
---|
| 193 | typedef enum {
|
---|
| 194 | MAVLINK_PARSE_STATE_UNINIT=0,
|
---|
| 195 | MAVLINK_PARSE_STATE_IDLE,
|
---|
| 196 | MAVLINK_PARSE_STATE_GOT_STX,
|
---|
| 197 | MAVLINK_PARSE_STATE_GOT_SEQ,
|
---|
| 198 | MAVLINK_PARSE_STATE_GOT_LENGTH,
|
---|
| 199 | MAVLINK_PARSE_STATE_GOT_SYSID,
|
---|
| 200 | MAVLINK_PARSE_STATE_GOT_COMPID,
|
---|
| 201 | MAVLINK_PARSE_STATE_GOT_MSGID,
|
---|
| 202 | MAVLINK_PARSE_STATE_GOT_PAYLOAD,
|
---|
| 203 | MAVLINK_PARSE_STATE_GOT_CRC1,
|
---|
| 204 | MAVLINK_PARSE_STATE_GOT_BAD_CRC1
|
---|
| 205 | } mavlink_parse_state_t; ///< The state machine for the comm parser
|
---|
| 206 |
|
---|
| 207 | typedef enum {
|
---|
| 208 | MAVLINK_FRAMING_INCOMPLETE=0,
|
---|
| 209 | MAVLINK_FRAMING_OK=1,
|
---|
| 210 | MAVLINK_FRAMING_BAD_CRC=2
|
---|
| 211 | } mavlink_framing_t;
|
---|
| 212 |
|
---|
| 213 | typedef struct __mavlink_status {
|
---|
| 214 | uint8_t msg_received; ///< Number of received messages
|
---|
| 215 | uint8_t buffer_overrun; ///< Number of buffer overruns
|
---|
| 216 | uint8_t parse_error; ///< Number of parse errors
|
---|
| 217 | mavlink_parse_state_t parse_state; ///< Parsing state machine
|
---|
| 218 | uint8_t packet_idx; ///< Index in current packet
|
---|
| 219 | uint8_t current_rx_seq; ///< Sequence number of last packet received
|
---|
| 220 | uint8_t current_tx_seq; ///< Sequence number of last packet sent
|
---|
| 221 | uint16_t packet_rx_success_count; ///< Received packets
|
---|
| 222 | uint16_t packet_rx_drop_count; ///< Number of packet drops
|
---|
| 223 | } mavlink_status_t;
|
---|
| 224 |
|
---|
| 225 | #define MAVLINK_BIG_ENDIAN 0
|
---|
| 226 | #define MAVLINK_LITTLE_ENDIAN 1
|
---|
| 227 |
|
---|
| 228 | #endif /* MAVLINK_TYPES_H_ */
|
---|