pinxian wu
mavlink library
Dependents: mavlink F429ZI_LCD_demo
Fork of
mavlink_bridge
by 
Benjamin Hepp
mavlink/v10/mavlink_helpers.h@11:76345550efb4, 2018-07-23 (annotated)
- Committer:
- wupinxian
- Date:
- Mon Jul 23 16:34:26 2018 +0000
- Revision:
- 11:76345550efb4
- Parent:
- 0:28183cc7963f
mavlink for stm32 Nucleo
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| bhepp | 0:28183cc7963f | 1 | #ifndef _MAVLINK_HELPERS_H_ |
| bhepp | 0:28183cc7963f | 2 | #define _MAVLINK_HELPERS_H_ |
| bhepp | 0:28183cc7963f | 3 | |
| bhepp | 0:28183cc7963f | 4 | #include "string.h" |
| bhepp | 0:28183cc7963f | 5 | #include "checksum.h" |
| bhepp | 0:28183cc7963f | 6 | #include "mavlink_types.h" |
| bhepp | 0:28183cc7963f | 7 | #include "mavlink_conversions.h" |
| bhepp | 0:28183cc7963f | 8 | |
| bhepp | 0:28183cc7963f | 9 | #ifndef MAVLINK_HELPER |
| bhepp | 0:28183cc7963f | 10 | #define MAVLINK_HELPER |
| bhepp | 0:28183cc7963f | 11 | #endif |
| bhepp | 0:28183cc7963f | 12 | |
| bhepp | 0:28183cc7963f | 13 | /* |
| bhepp | 0:28183cc7963f | 14 | * Internal function to give access to the channel status for each channel |
| bhepp | 0:28183cc7963f | 15 | */ |
| bhepp | 0:28183cc7963f | 16 | #ifndef MAVLINK_GET_CHANNEL_STATUS |
| bhepp | 0:28183cc7963f | 17 | MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) |
| bhepp | 0:28183cc7963f | 18 | { |
| bhepp | 0:28183cc7963f | 19 | #ifdef MAVLINK_EXTERNAL_RX_STATUS |
| bhepp | 0:28183cc7963f | 20 | // No m_mavlink_status array defined in function, |
| bhepp | 0:28183cc7963f | 21 | // has to be defined externally |
| bhepp | 0:28183cc7963f | 22 | #else |
| bhepp | 0:28183cc7963f | 23 | static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; |
| bhepp | 0:28183cc7963f | 24 | #endif |
| bhepp | 0:28183cc7963f | 25 | return &m_mavlink_status[chan]; |
| bhepp | 0:28183cc7963f | 26 | } |
| bhepp | 0:28183cc7963f | 27 | #endif |
| bhepp | 0:28183cc7963f | 28 | |
| bhepp | 0:28183cc7963f | 29 | /* |
| bhepp | 0:28183cc7963f | 30 | * Internal function to give access to the channel buffer for each channel |
| bhepp | 0:28183cc7963f | 31 | */ |
| bhepp | 0:28183cc7963f | 32 | #ifndef MAVLINK_GET_CHANNEL_BUFFER |
| bhepp | 0:28183cc7963f | 33 | MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) |
| bhepp | 0:28183cc7963f | 34 | { |
| bhepp | 0:28183cc7963f | 35 | |
| bhepp | 0:28183cc7963f | 36 | #ifdef MAVLINK_EXTERNAL_RX_BUFFER |
| bhepp | 0:28183cc7963f | 37 | // No m_mavlink_buffer array defined in function, |
| bhepp | 0:28183cc7963f | 38 | // has to be defined externally |
| bhepp | 0:28183cc7963f | 39 | #else |
| bhepp | 0:28183cc7963f | 40 | static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; |
| bhepp | 0:28183cc7963f | 41 | #endif |
| bhepp | 0:28183cc7963f | 42 | return &m_mavlink_buffer[chan]; |
| bhepp | 0:28183cc7963f | 43 | } |
| bhepp | 0:28183cc7963f | 44 | #endif |
| bhepp | 0:28183cc7963f | 45 | |
| bhepp | 0:28183cc7963f | 46 | /** |
| bhepp | 0:28183cc7963f | 47 | * @brief Reset the status of a channel. |
| bhepp | 0:28183cc7963f | 48 | */ |
| bhepp | 0:28183cc7963f | 49 | MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) |
| bhepp | 0:28183cc7963f | 50 | { |
| bhepp | 0:28183cc7963f | 51 | mavlink_status_t *status = mavlink_get_channel_status(chan); |
| bhepp | 0:28183cc7963f | 52 | status->parse_state = MAVLINK_PARSE_STATE_IDLE; |
| bhepp | 0:28183cc7963f | 53 | } |
| bhepp | 0:28183cc7963f | 54 | |
| bhepp | 0:28183cc7963f | 55 | /** |
| bhepp | 0:28183cc7963f | 56 | * @brief Finalize a MAVLink message with channel assignment |
| bhepp | 0:28183cc7963f | 57 | * |
| bhepp | 0:28183cc7963f | 58 | * This function calculates the checksum and sets length and aircraft id correctly. |
| bhepp | 0:28183cc7963f | 59 | * It assumes that the message id and the payload are already correctly set. This function |
| bhepp | 0:28183cc7963f | 60 | * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack |
| bhepp | 0:28183cc7963f | 61 | * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. |
| bhepp | 0:28183cc7963f | 62 | * |
| bhepp | 0:28183cc7963f | 63 | * @param msg Message to finalize |
| bhepp | 0:28183cc7963f | 64 | * @param system_id Id of the sending (this) system, 1-127 |
| bhepp | 0:28183cc7963f | 65 | * @param length Message length |
| bhepp | 0:28183cc7963f | 66 | */ |
| bhepp | 0:28183cc7963f | 67 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 68 | MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, |
| bhepp | 0:28183cc7963f | 69 | uint8_t chan, uint8_t length, uint8_t crc_extra) |
| bhepp | 0:28183cc7963f | 70 | #else |
| bhepp | 0:28183cc7963f | 71 | MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, |
| bhepp | 0:28183cc7963f | 72 | uint8_t chan, uint8_t length) |
| bhepp | 0:28183cc7963f | 73 | #endif |
| bhepp | 0:28183cc7963f | 74 | { |
| bhepp | 0:28183cc7963f | 75 | // This code part is the same for all messages; |
| bhepp | 0:28183cc7963f | 76 | msg->magic = MAVLINK_STX; |
| bhepp | 0:28183cc7963f | 77 | msg->len = length; |
| bhepp | 0:28183cc7963f | 78 | msg->sysid = system_id; |
| bhepp | 0:28183cc7963f | 79 | msg->compid = component_id; |
| bhepp | 0:28183cc7963f | 80 | // One sequence number per channel |
| bhepp | 0:28183cc7963f | 81 | msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; |
| bhepp | 0:28183cc7963f | 82 | mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; |
| bhepp | 0:28183cc7963f | 83 | msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); |
| bhepp | 0:28183cc7963f | 84 | crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); |
| bhepp | 0:28183cc7963f | 85 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 86 | crc_accumulate(crc_extra, &msg->checksum); |
| bhepp | 0:28183cc7963f | 87 | #endif |
| bhepp | 0:28183cc7963f | 88 | mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); |
| bhepp | 0:28183cc7963f | 89 | mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); |
| bhepp | 0:28183cc7963f | 90 | |
| bhepp | 0:28183cc7963f | 91 | return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; |
| bhepp | 0:28183cc7963f | 92 | } |
| bhepp | 0:28183cc7963f | 93 | |
| bhepp | 0:28183cc7963f | 94 | |
| bhepp | 0:28183cc7963f | 95 | /** |
| bhepp | 0:28183cc7963f | 96 | * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel |
| bhepp | 0:28183cc7963f | 97 | */ |
| bhepp | 0:28183cc7963f | 98 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 99 | MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, |
| bhepp | 0:28183cc7963f | 100 | uint8_t length, uint8_t crc_extra) |
| bhepp | 0:28183cc7963f | 101 | { |
| bhepp | 0:28183cc7963f | 102 | return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); |
| bhepp | 0:28183cc7963f | 103 | } |
| bhepp | 0:28183cc7963f | 104 | #else |
| bhepp | 0:28183cc7963f | 105 | MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, |
| bhepp | 0:28183cc7963f | 106 | uint8_t length) |
| bhepp | 0:28183cc7963f | 107 | { |
| bhepp | 0:28183cc7963f | 108 | return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); |
| bhepp | 0:28183cc7963f | 109 | } |
| bhepp | 0:28183cc7963f | 110 | #endif |
| bhepp | 0:28183cc7963f | 111 | |
| bhepp | 0:28183cc7963f | 112 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS |
| bhepp | 0:28183cc7963f | 113 | MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); |
| bhepp | 0:28183cc7963f | 114 | |
| bhepp | 0:28183cc7963f | 115 | /** |
| bhepp | 0:28183cc7963f | 116 | * @brief Finalize a MAVLink message with channel assignment and send |
| bhepp | 0:28183cc7963f | 117 | */ |
| bhepp | 0:28183cc7963f | 118 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 119 | MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, |
| bhepp | 0:28183cc7963f | 120 | uint8_t length, uint8_t crc_extra) |
| bhepp | 0:28183cc7963f | 121 | #else |
| bhepp | 0:28183cc7963f | 122 | MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) |
| bhepp | 0:28183cc7963f | 123 | #endif |
| bhepp | 0:28183cc7963f | 124 | { |
| bhepp | 0:28183cc7963f | 125 | uint16_t checksum; |
| bhepp | 0:28183cc7963f | 126 | uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; |
| bhepp | 0:28183cc7963f | 127 | uint8_t ck[2]; |
| bhepp | 0:28183cc7963f | 128 | mavlink_status_t *status = mavlink_get_channel_status(chan); |
| bhepp | 0:28183cc7963f | 129 | buf[0] = MAVLINK_STX; |
| bhepp | 0:28183cc7963f | 130 | buf[1] = length; |
| bhepp | 0:28183cc7963f | 131 | buf[2] = status->current_tx_seq; |
| bhepp | 0:28183cc7963f | 132 | buf[3] = mavlink_system.sysid; |
| bhepp | 0:28183cc7963f | 133 | buf[4] = mavlink_system.compid; |
| bhepp | 0:28183cc7963f | 134 | buf[5] = msgid; |
| bhepp | 0:28183cc7963f | 135 | status->current_tx_seq++; |
| bhepp | 0:28183cc7963f | 136 | checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); |
| bhepp | 0:28183cc7963f | 137 | crc_accumulate_buffer(&checksum, packet, length); |
| bhepp | 0:28183cc7963f | 138 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 139 | crc_accumulate(crc_extra, &checksum); |
| bhepp | 0:28183cc7963f | 140 | #endif |
| bhepp | 0:28183cc7963f | 141 | ck[0] = (uint8_t)(checksum & 0xFF); |
| bhepp | 0:28183cc7963f | 142 | ck[1] = (uint8_t)(checksum >> 8); |
| bhepp | 0:28183cc7963f | 143 | |
| bhepp | 0:28183cc7963f | 144 | MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); |
| bhepp | 0:28183cc7963f | 145 | _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); |
| bhepp | 0:28183cc7963f | 146 | _mavlink_send_uart(chan, packet, length); |
| bhepp | 0:28183cc7963f | 147 | _mavlink_send_uart(chan, (const char *)ck, 2); |
| bhepp | 0:28183cc7963f | 148 | MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); |
| bhepp | 0:28183cc7963f | 149 | } |
| bhepp | 0:28183cc7963f | 150 | |
| bhepp | 0:28183cc7963f | 151 | /** |
| bhepp | 0:28183cc7963f | 152 | * @brief re-send a message over a uart channel |
| bhepp | 0:28183cc7963f | 153 | * this is more stack efficient than re-marshalling the message |
| bhepp | 0:28183cc7963f | 154 | */ |
| bhepp | 0:28183cc7963f | 155 | MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) |
| bhepp | 0:28183cc7963f | 156 | { |
| bhepp | 0:28183cc7963f | 157 | uint8_t ck[2]; |
| bhepp | 0:28183cc7963f | 158 | |
| bhepp | 0:28183cc7963f | 159 | ck[0] = (uint8_t)(msg->checksum & 0xFF); |
| bhepp | 0:28183cc7963f | 160 | ck[1] = (uint8_t)(msg->checksum >> 8); |
| bhepp | 0:28183cc7963f | 161 | // XXX use the right sequence here |
| bhepp | 0:28183cc7963f | 162 | |
| bhepp | 0:28183cc7963f | 163 | MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); |
| bhepp | 0:28183cc7963f | 164 | _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); |
| bhepp | 0:28183cc7963f | 165 | _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); |
| bhepp | 0:28183cc7963f | 166 | _mavlink_send_uart(chan, (const char *)ck, 2); |
| bhepp | 0:28183cc7963f | 167 | MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); |
| bhepp | 0:28183cc7963f | 168 | } |
| bhepp | 0:28183cc7963f | 169 | #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS |
| bhepp | 0:28183cc7963f | 170 | |
| bhepp | 0:28183cc7963f | 171 | /** |
| bhepp | 0:28183cc7963f | 172 | * @brief Pack a message to send it over a serial byte stream |
| bhepp | 0:28183cc7963f | 173 | */ |
| bhepp | 0:28183cc7963f | 174 | MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) |
| bhepp | 0:28183cc7963f | 175 | { |
| bhepp | 0:28183cc7963f | 176 | memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); |
| bhepp | 0:28183cc7963f | 177 | |
| bhepp | 0:28183cc7963f | 178 | uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); |
| bhepp | 0:28183cc7963f | 179 | |
| bhepp | 0:28183cc7963f | 180 | ck[0] = (uint8_t)(msg->checksum & 0xFF); |
| bhepp | 0:28183cc7963f | 181 | ck[1] = (uint8_t)(msg->checksum >> 8); |
| bhepp | 0:28183cc7963f | 182 | |
| bhepp | 0:28183cc7963f | 183 | return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; |
| bhepp | 0:28183cc7963f | 184 | } |
| bhepp | 0:28183cc7963f | 185 | |
| bhepp | 0:28183cc7963f | 186 | union __mavlink_bitfield { |
| bhepp | 0:28183cc7963f | 187 | uint8_t uint8; |
| bhepp | 0:28183cc7963f | 188 | int8_t int8; |
| bhepp | 0:28183cc7963f | 189 | uint16_t uint16; |
| bhepp | 0:28183cc7963f | 190 | int16_t int16; |
| bhepp | 0:28183cc7963f | 191 | uint32_t uint32; |
| bhepp | 0:28183cc7963f | 192 | int32_t int32; |
| bhepp | 0:28183cc7963f | 193 | }; |
| bhepp | 0:28183cc7963f | 194 | |
| bhepp | 0:28183cc7963f | 195 | |
| bhepp | 0:28183cc7963f | 196 | MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) |
| bhepp | 0:28183cc7963f | 197 | { |
| bhepp | 0:28183cc7963f | 198 | crc_init(&msg->checksum); |
| bhepp | 0:28183cc7963f | 199 | } |
| bhepp | 0:28183cc7963f | 200 | |
| bhepp | 0:28183cc7963f | 201 | MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) |
| bhepp | 0:28183cc7963f | 202 | { |
| bhepp | 0:28183cc7963f | 203 | crc_accumulate(c, &msg->checksum); |
| bhepp | 0:28183cc7963f | 204 | } |
| bhepp | 0:28183cc7963f | 205 | |
| bhepp | 0:28183cc7963f | 206 | /** |
| bhepp | 0:28183cc7963f | 207 | * This is a varient of mavlink_frame_char() but with caller supplied |
| bhepp | 0:28183cc7963f | 208 | * parsing buffers. It is useful when you want to create a MAVLink |
| bhepp | 0:28183cc7963f | 209 | * parser in a library that doesn't use any global variables |
| bhepp | 0:28183cc7963f | 210 | * |
| bhepp | 0:28183cc7963f | 211 | * @param rxmsg parsing message buffer |
| bhepp | 0:28183cc7963f | 212 | * @param status parsing starus buffer |
| bhepp | 0:28183cc7963f | 213 | * @param c The char to parse |
| bhepp | 0:28183cc7963f | 214 | * |
| bhepp | 0:28183cc7963f | 215 | * @param returnMsg NULL if no message could be decoded, the message data else |
| bhepp | 0:28183cc7963f | 216 | * @param returnStats if a message was decoded, this is filled with the channel's stats |
| bhepp | 0:28183cc7963f | 217 | * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC |
| bhepp | 0:28183cc7963f | 218 | * |
| bhepp | 0:28183cc7963f | 219 | * A typical use scenario of this function call is: |
| bhepp | 0:28183cc7963f | 220 | * |
| bhepp | 0:28183cc7963f | 221 | * @code |
| bhepp | 0:28183cc7963f | 222 | * #include <mavlink.h> |
| bhepp | 0:28183cc7963f | 223 | * |
| bhepp | 0:28183cc7963f | 224 | * mavlink_message_t msg; |
| bhepp | 0:28183cc7963f | 225 | * int chan = 0; |
| bhepp | 0:28183cc7963f | 226 | * |
| bhepp | 0:28183cc7963f | 227 | * |
| bhepp | 0:28183cc7963f | 228 | * while(serial.bytesAvailable > 0) |
| bhepp | 0:28183cc7963f | 229 | * { |
| bhepp | 0:28183cc7963f | 230 | * uint8_t byte = serial.getNextByte(); |
| bhepp | 0:28183cc7963f | 231 | * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) |
| bhepp | 0:28183cc7963f | 232 | * { |
| bhepp | 0:28183cc7963f | 233 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); |
| bhepp | 0:28183cc7963f | 234 | * } |
| bhepp | 0:28183cc7963f | 235 | * } |
| bhepp | 0:28183cc7963f | 236 | * |
| bhepp | 0:28183cc7963f | 237 | * |
| bhepp | 0:28183cc7963f | 238 | * @endcode |
| bhepp | 0:28183cc7963f | 239 | */ |
| bhepp | 0:28183cc7963f | 240 | MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, |
| bhepp | 0:28183cc7963f | 241 | mavlink_status_t* status, |
| bhepp | 0:28183cc7963f | 242 | uint8_t c, |
| bhepp | 0:28183cc7963f | 243 | mavlink_message_t* r_message, |
| bhepp | 0:28183cc7963f | 244 | mavlink_status_t* r_mavlink_status) |
| bhepp | 0:28183cc7963f | 245 | { |
| bhepp | 0:28183cc7963f | 246 | /* |
| bhepp | 0:28183cc7963f | 247 | default message crc function. You can override this per-system to |
| bhepp | 0:28183cc7963f | 248 | put this data in a different memory segment |
| bhepp | 0:28183cc7963f | 249 | */ |
| bhepp | 0:28183cc7963f | 250 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 251 | #ifndef MAVLINK_MESSAGE_CRC |
| bhepp | 0:28183cc7963f | 252 | static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; |
| bhepp | 0:28183cc7963f | 253 | #define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] |
| bhepp | 0:28183cc7963f | 254 | #endif |
| bhepp | 0:28183cc7963f | 255 | #endif |
| bhepp | 0:28183cc7963f | 256 | |
| bhepp | 0:28183cc7963f | 257 | /* Enable this option to check the length of each message. |
| bhepp | 0:28183cc7963f | 258 | This allows invalid messages to be caught much sooner. Use if the transmission |
| bhepp | 0:28183cc7963f | 259 | medium is prone to missing (or extra) characters (e.g. a radio that fades in |
| bhepp | 0:28183cc7963f | 260 | and out). Only use if the channel will only contain messages types listed in |
| bhepp | 0:28183cc7963f | 261 | the headers. |
| bhepp | 0:28183cc7963f | 262 | */ |
| bhepp | 0:28183cc7963f | 263 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH |
| bhepp | 0:28183cc7963f | 264 | #ifndef MAVLINK_MESSAGE_LENGTH |
| bhepp | 0:28183cc7963f | 265 | static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; |
| bhepp | 0:28183cc7963f | 266 | #define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] |
| bhepp | 0:28183cc7963f | 267 | #endif |
| bhepp | 0:28183cc7963f | 268 | #endif |
| bhepp | 0:28183cc7963f | 269 | |
| bhepp | 0:28183cc7963f | 270 | int bufferIndex = 0; |
| bhepp | 0:28183cc7963f | 271 | |
| bhepp | 0:28183cc7963f | 272 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE; |
| bhepp | 0:28183cc7963f | 273 | |
| bhepp | 0:28183cc7963f | 274 | switch (status->parse_state) |
| bhepp | 0:28183cc7963f | 275 | { |
| bhepp | 0:28183cc7963f | 276 | case MAVLINK_PARSE_STATE_UNINIT: |
| bhepp | 0:28183cc7963f | 277 | case MAVLINK_PARSE_STATE_IDLE: |
| bhepp | 0:28183cc7963f | 278 | if (c == MAVLINK_STX) |
| bhepp | 0:28183cc7963f | 279 | { |
| bhepp | 0:28183cc7963f | 280 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; |
| bhepp | 0:28183cc7963f | 281 | rxmsg->len = 0; |
| bhepp | 0:28183cc7963f | 282 | rxmsg->magic = c; |
| bhepp | 0:28183cc7963f | 283 | mavlink_start_checksum(rxmsg); |
| bhepp | 0:28183cc7963f | 284 | } |
| bhepp | 0:28183cc7963f | 285 | break; |
| bhepp | 0:28183cc7963f | 286 | |
| bhepp | 0:28183cc7963f | 287 | case MAVLINK_PARSE_STATE_GOT_STX: |
| bhepp | 0:28183cc7963f | 288 | if (status->msg_received |
| bhepp | 0:28183cc7963f | 289 | /* Support shorter buffers than the |
| bhepp | 0:28183cc7963f | 290 | default maximum packet size */ |
| bhepp | 0:28183cc7963f | 291 | #if (MAVLINK_MAX_PAYLOAD_LEN < 255) |
| bhepp | 0:28183cc7963f | 292 | || c > MAVLINK_MAX_PAYLOAD_LEN |
| bhepp | 0:28183cc7963f | 293 | #endif |
| bhepp | 0:28183cc7963f | 294 | ) |
| bhepp | 0:28183cc7963f | 295 | { |
| bhepp | 0:28183cc7963f | 296 | status->buffer_overrun++; |
| bhepp | 0:28183cc7963f | 297 | status->parse_error++; |
| bhepp | 0:28183cc7963f | 298 | status->msg_received = 0; |
| bhepp | 0:28183cc7963f | 299 | status->parse_state = MAVLINK_PARSE_STATE_IDLE; |
| bhepp | 0:28183cc7963f | 300 | } |
| bhepp | 0:28183cc7963f | 301 | else |
| bhepp | 0:28183cc7963f | 302 | { |
| bhepp | 0:28183cc7963f | 303 | // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 |
| bhepp | 0:28183cc7963f | 304 | rxmsg->len = c; |
| bhepp | 0:28183cc7963f | 305 | status->packet_idx = 0; |
| bhepp | 0:28183cc7963f | 306 | mavlink_update_checksum(rxmsg, c); |
| bhepp | 0:28183cc7963f | 307 | status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; |
| bhepp | 0:28183cc7963f | 308 | } |
| bhepp | 0:28183cc7963f | 309 | break; |
| bhepp | 0:28183cc7963f | 310 | |
| bhepp | 0:28183cc7963f | 311 | case MAVLINK_PARSE_STATE_GOT_LENGTH: |
| bhepp | 0:28183cc7963f | 312 | rxmsg->seq = c; |
| bhepp | 0:28183cc7963f | 313 | mavlink_update_checksum(rxmsg, c); |
| bhepp | 0:28183cc7963f | 314 | status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; |
| bhepp | 0:28183cc7963f | 315 | break; |
| bhepp | 0:28183cc7963f | 316 | |
| bhepp | 0:28183cc7963f | 317 | case MAVLINK_PARSE_STATE_GOT_SEQ: |
| bhepp | 0:28183cc7963f | 318 | rxmsg->sysid = c; |
| bhepp | 0:28183cc7963f | 319 | mavlink_update_checksum(rxmsg, c); |
| bhepp | 0:28183cc7963f | 320 | status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; |
| bhepp | 0:28183cc7963f | 321 | break; |
| bhepp | 0:28183cc7963f | 322 | |
| bhepp | 0:28183cc7963f | 323 | case MAVLINK_PARSE_STATE_GOT_SYSID: |
| bhepp | 0:28183cc7963f | 324 | rxmsg->compid = c; |
| bhepp | 0:28183cc7963f | 325 | mavlink_update_checksum(rxmsg, c); |
| bhepp | 0:28183cc7963f | 326 | status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; |
| bhepp | 0:28183cc7963f | 327 | break; |
| bhepp | 0:28183cc7963f | 328 | |
| bhepp | 0:28183cc7963f | 329 | case MAVLINK_PARSE_STATE_GOT_COMPID: |
| bhepp | 0:28183cc7963f | 330 | #ifdef MAVLINK_CHECK_MESSAGE_LENGTH |
| bhepp | 0:28183cc7963f | 331 | if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c)) |
| bhepp | 0:28183cc7963f | 332 | { |
| bhepp | 0:28183cc7963f | 333 | status->parse_error++; |
| bhepp | 0:28183cc7963f | 334 | status->parse_state = MAVLINK_PARSE_STATE_IDLE; |
| bhepp | 0:28183cc7963f | 335 | break; |
| bhepp | 0:28183cc7963f | 336 | } |
| bhepp | 0:28183cc7963f | 337 | #endif |
| bhepp | 0:28183cc7963f | 338 | rxmsg->msgid = c; |
| bhepp | 0:28183cc7963f | 339 | mavlink_update_checksum(rxmsg, c); |
| bhepp | 0:28183cc7963f | 340 | if (rxmsg->len == 0) |
| bhepp | 0:28183cc7963f | 341 | { |
| bhepp | 0:28183cc7963f | 342 | status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; |
| bhepp | 0:28183cc7963f | 343 | } |
| bhepp | 0:28183cc7963f | 344 | else |
| bhepp | 0:28183cc7963f | 345 | { |
| bhepp | 0:28183cc7963f | 346 | status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; |
| bhepp | 0:28183cc7963f | 347 | } |
| bhepp | 0:28183cc7963f | 348 | break; |
| bhepp | 0:28183cc7963f | 349 | |
| bhepp | 0:28183cc7963f | 350 | case MAVLINK_PARSE_STATE_GOT_MSGID: |
| bhepp | 0:28183cc7963f | 351 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; |
| bhepp | 0:28183cc7963f | 352 | mavlink_update_checksum(rxmsg, c); |
| bhepp | 0:28183cc7963f | 353 | if (status->packet_idx == rxmsg->len) |
| bhepp | 0:28183cc7963f | 354 | { |
| bhepp | 0:28183cc7963f | 355 | status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; |
| bhepp | 0:28183cc7963f | 356 | } |
| bhepp | 0:28183cc7963f | 357 | break; |
| bhepp | 0:28183cc7963f | 358 | |
| bhepp | 0:28183cc7963f | 359 | case MAVLINK_PARSE_STATE_GOT_PAYLOAD: |
| bhepp | 0:28183cc7963f | 360 | #if MAVLINK_CRC_EXTRA |
| bhepp | 0:28183cc7963f | 361 | mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); |
| bhepp | 0:28183cc7963f | 362 | #endif |
| bhepp | 0:28183cc7963f | 363 | if (c != (rxmsg->checksum & 0xFF)) { |
| bhepp | 0:28183cc7963f | 364 | status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; |
| bhepp | 0:28183cc7963f | 365 | } else { |
| bhepp | 0:28183cc7963f | 366 | status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; |
| bhepp | 0:28183cc7963f | 367 | } |
| bhepp | 0:28183cc7963f | 368 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; |
| bhepp | 0:28183cc7963f | 369 | break; |
| bhepp | 0:28183cc7963f | 370 | |
| bhepp | 0:28183cc7963f | 371 | case MAVLINK_PARSE_STATE_GOT_CRC1: |
| bhepp | 0:28183cc7963f | 372 | case MAVLINK_PARSE_STATE_GOT_BAD_CRC1: |
| bhepp | 0:28183cc7963f | 373 | if (status->parse_state == MAVLINK_PARSE_STATE_GOT_BAD_CRC1 || c != (rxmsg->checksum >> 8)) { |
| bhepp | 0:28183cc7963f | 374 | // got a bad CRC message |
| bhepp | 0:28183cc7963f | 375 | status->msg_received = MAVLINK_FRAMING_BAD_CRC; |
| bhepp | 0:28183cc7963f | 376 | } else { |
| bhepp | 0:28183cc7963f | 377 | // Successfully got message |
| bhepp | 0:28183cc7963f | 378 | status->msg_received = MAVLINK_FRAMING_OK; |
| bhepp | 0:28183cc7963f | 379 | } |
| bhepp | 0:28183cc7963f | 380 | status->parse_state = MAVLINK_PARSE_STATE_IDLE; |
| bhepp | 0:28183cc7963f | 381 | _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; |
| bhepp | 0:28183cc7963f | 382 | memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); |
| bhepp | 0:28183cc7963f | 383 | break; |
| bhepp | 0:28183cc7963f | 384 | } |
| bhepp | 0:28183cc7963f | 385 | |
| bhepp | 0:28183cc7963f | 386 | bufferIndex++; |
| bhepp | 0:28183cc7963f | 387 | // If a message has been sucessfully decoded, check index |
| bhepp | 0:28183cc7963f | 388 | if (status->msg_received == MAVLINK_FRAMING_OK) |
| bhepp | 0:28183cc7963f | 389 | { |
| bhepp | 0:28183cc7963f | 390 | //while(status->current_seq != rxmsg->seq) |
| bhepp | 0:28183cc7963f | 391 | //{ |
| bhepp | 0:28183cc7963f | 392 | // status->packet_rx_drop_count++; |
| bhepp | 0:28183cc7963f | 393 | // status->current_seq++; |
| bhepp | 0:28183cc7963f | 394 | //} |
| bhepp | 0:28183cc7963f | 395 | status->current_rx_seq = rxmsg->seq; |
| bhepp | 0:28183cc7963f | 396 | // Initial condition: If no packet has been received so far, drop count is undefined |
| bhepp | 0:28183cc7963f | 397 | if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; |
| bhepp | 0:28183cc7963f | 398 | // Count this packet as received |
| bhepp | 0:28183cc7963f | 399 | status->packet_rx_success_count++; |
| bhepp | 0:28183cc7963f | 400 | } |
| bhepp | 0:28183cc7963f | 401 | |
| bhepp | 0:28183cc7963f | 402 | r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg |
| bhepp | 0:28183cc7963f | 403 | r_mavlink_status->parse_state = status->parse_state; |
| bhepp | 0:28183cc7963f | 404 | r_mavlink_status->packet_idx = status->packet_idx; |
| bhepp | 0:28183cc7963f | 405 | r_mavlink_status->current_rx_seq = status->current_rx_seq+1; |
| bhepp | 0:28183cc7963f | 406 | r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; |
| bhepp | 0:28183cc7963f | 407 | r_mavlink_status->packet_rx_drop_count = status->parse_error; |
| bhepp | 0:28183cc7963f | 408 | status->parse_error = 0; |
| bhepp | 0:28183cc7963f | 409 | |
| bhepp | 0:28183cc7963f | 410 | if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { |
| bhepp | 0:28183cc7963f | 411 | /* |
| bhepp | 0:28183cc7963f | 412 | the CRC came out wrong. We now need to overwrite the |
| bhepp | 0:28183cc7963f | 413 | msg CRC with the one on the wire so that if the |
| bhepp | 0:28183cc7963f | 414 | caller decides to forward the message anyway that |
| bhepp | 0:28183cc7963f | 415 | mavlink_msg_to_send_buffer() won't overwrite the |
| bhepp | 0:28183cc7963f | 416 | checksum |
| bhepp | 0:28183cc7963f | 417 | */ |
| bhepp | 0:28183cc7963f | 418 | r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8); |
| bhepp | 0:28183cc7963f | 419 | } |
| bhepp | 0:28183cc7963f | 420 | |
| bhepp | 0:28183cc7963f | 421 | return status->msg_received; |
| bhepp | 0:28183cc7963f | 422 | } |
| bhepp | 0:28183cc7963f | 423 | |
| bhepp | 0:28183cc7963f | 424 | /** |
| bhepp | 0:28183cc7963f | 425 | * This is a convenience function which handles the complete MAVLink parsing. |
| bhepp | 0:28183cc7963f | 426 | * the function will parse one byte at a time and return the complete packet once |
| bhepp | 0:28183cc7963f | 427 | * it could be successfully decoded. This function will return 0, 1 or |
| bhepp | 0:28183cc7963f | 428 | * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC) |
| bhepp | 0:28183cc7963f | 429 | * |
| bhepp | 0:28183cc7963f | 430 | * Messages are parsed into an internal buffer (one for each channel). When a complete |
| bhepp | 0:28183cc7963f | 431 | * message is received it is copies into *returnMsg and the channel's status is |
| bhepp | 0:28183cc7963f | 432 | * copied into *returnStats. |
| bhepp | 0:28183cc7963f | 433 | * |
| bhepp | 0:28183cc7963f | 434 | * @param chan ID of the current channel. This allows to parse different channels with this function. |
| bhepp | 0:28183cc7963f | 435 | * a channel is not a physical message channel like a serial port, but a logic partition of |
| bhepp | 0:28183cc7963f | 436 | * the communication streams in this case. COMM_NB is the limit for the number of channels |
| bhepp | 0:28183cc7963f | 437 | * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows |
| bhepp | 0:28183cc7963f | 438 | * @param c The char to parse |
| bhepp | 0:28183cc7963f | 439 | * |
| bhepp | 0:28183cc7963f | 440 | * @param returnMsg NULL if no message could be decoded, the message data else |
| bhepp | 0:28183cc7963f | 441 | * @param returnStats if a message was decoded, this is filled with the channel's stats |
| bhepp | 0:28183cc7963f | 442 | * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC |
| bhepp | 0:28183cc7963f | 443 | * |
| bhepp | 0:28183cc7963f | 444 | * A typical use scenario of this function call is: |
| bhepp | 0:28183cc7963f | 445 | * |
| bhepp | 0:28183cc7963f | 446 | * @code |
| bhepp | 0:28183cc7963f | 447 | * #include <mavlink.h> |
| bhepp | 0:28183cc7963f | 448 | * |
| bhepp | 0:28183cc7963f | 449 | * mavlink_message_t msg; |
| bhepp | 0:28183cc7963f | 450 | * int chan = 0; |
| bhepp | 0:28183cc7963f | 451 | * |
| bhepp | 0:28183cc7963f | 452 | * |
| bhepp | 0:28183cc7963f | 453 | * while(serial.bytesAvailable > 0) |
| bhepp | 0:28183cc7963f | 454 | * { |
| bhepp | 0:28183cc7963f | 455 | * uint8_t byte = serial.getNextByte(); |
| bhepp | 0:28183cc7963f | 456 | * if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE) |
| bhepp | 0:28183cc7963f | 457 | * { |
| bhepp | 0:28183cc7963f | 458 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); |
| bhepp | 0:28183cc7963f | 459 | * } |
| bhepp | 0:28183cc7963f | 460 | * } |
| bhepp | 0:28183cc7963f | 461 | * |
| bhepp | 0:28183cc7963f | 462 | * |
| bhepp | 0:28183cc7963f | 463 | * @endcode |
| bhepp | 0:28183cc7963f | 464 | */ |
| bhepp | 0:28183cc7963f | 465 | MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) |
| bhepp | 0:28183cc7963f | 466 | { |
| bhepp | 0:28183cc7963f | 467 | return mavlink_frame_char_buffer(mavlink_get_channel_buffer(chan), |
| bhepp | 0:28183cc7963f | 468 | mavlink_get_channel_status(chan), |
| bhepp | 0:28183cc7963f | 469 | c, |
| bhepp | 0:28183cc7963f | 470 | r_message, |
| bhepp | 0:28183cc7963f | 471 | r_mavlink_status); |
| bhepp | 0:28183cc7963f | 472 | } |
| bhepp | 0:28183cc7963f | 473 | |
| bhepp | 0:28183cc7963f | 474 | |
| bhepp | 0:28183cc7963f | 475 | /** |
| bhepp | 0:28183cc7963f | 476 | * This is a convenience function which handles the complete MAVLink parsing. |
| bhepp | 0:28183cc7963f | 477 | * the function will parse one byte at a time and return the complete packet once |
| bhepp | 0:28183cc7963f | 478 | * it could be successfully decoded. This function will return 0 or 1. |
| bhepp | 0:28183cc7963f | 479 | * |
| bhepp | 0:28183cc7963f | 480 | * Messages are parsed into an internal buffer (one for each channel). When a complete |
| bhepp | 0:28183cc7963f | 481 | * message is received it is copies into *returnMsg and the channel's status is |
| bhepp | 0:28183cc7963f | 482 | * copied into *returnStats. |
| bhepp | 0:28183cc7963f | 483 | * |
| bhepp | 0:28183cc7963f | 484 | * @param chan ID of the current channel. This allows to parse different channels with this function. |
| bhepp | 0:28183cc7963f | 485 | * a channel is not a physical message channel like a serial port, but a logic partition of |
| bhepp | 0:28183cc7963f | 486 | * the communication streams in this case. COMM_NB is the limit for the number of channels |
| bhepp | 0:28183cc7963f | 487 | * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows |
| bhepp | 0:28183cc7963f | 488 | * @param c The char to parse |
| bhepp | 0:28183cc7963f | 489 | * |
| bhepp | 0:28183cc7963f | 490 | * @param returnMsg NULL if no message could be decoded, the message data else |
| bhepp | 0:28183cc7963f | 491 | * @param returnStats if a message was decoded, this is filled with the channel's stats |
| bhepp | 0:28183cc7963f | 492 | * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC |
| bhepp | 0:28183cc7963f | 493 | * |
| bhepp | 0:28183cc7963f | 494 | * A typical use scenario of this function call is: |
| bhepp | 0:28183cc7963f | 495 | * |
| bhepp | 0:28183cc7963f | 496 | * @code |
| bhepp | 0:28183cc7963f | 497 | * #include <mavlink.h> |
| bhepp | 0:28183cc7963f | 498 | * |
| bhepp | 0:28183cc7963f | 499 | * mavlink_message_t msg; |
| bhepp | 0:28183cc7963f | 500 | * int chan = 0; |
| bhepp | 0:28183cc7963f | 501 | * |
| bhepp | 0:28183cc7963f | 502 | * |
| bhepp | 0:28183cc7963f | 503 | * while(serial.bytesAvailable > 0) |
| bhepp | 0:28183cc7963f | 504 | * { |
| bhepp | 0:28183cc7963f | 505 | * uint8_t byte = serial.getNextByte(); |
| bhepp | 0:28183cc7963f | 506 | * if (mavlink_parse_char(chan, byte, &msg)) |
| bhepp | 0:28183cc7963f | 507 | * { |
| bhepp | 0:28183cc7963f | 508 | * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); |
| bhepp | 0:28183cc7963f | 509 | * } |
| bhepp | 0:28183cc7963f | 510 | * } |
| bhepp | 0:28183cc7963f | 511 | * |
| bhepp | 0:28183cc7963f | 512 | * |
| bhepp | 0:28183cc7963f | 513 | * @endcode |
| bhepp | 0:28183cc7963f | 514 | */ |
| bhepp | 0:28183cc7963f | 515 | MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) |
| bhepp | 0:28183cc7963f | 516 | { |
| bhepp | 0:28183cc7963f | 517 | uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status); |
| bhepp | 0:28183cc7963f | 518 | if (msg_received == MAVLINK_FRAMING_BAD_CRC) { |
| bhepp | 0:28183cc7963f | 519 | // we got a bad CRC. Treat as a parse failure |
| bhepp | 0:28183cc7963f | 520 | mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); |
| bhepp | 0:28183cc7963f | 521 | mavlink_status_t* status = mavlink_get_channel_status(chan); |
| bhepp | 0:28183cc7963f | 522 | status->parse_error++; |
| bhepp | 0:28183cc7963f | 523 | status->msg_received = MAVLINK_FRAMING_INCOMPLETE; |
| bhepp | 0:28183cc7963f | 524 | status->parse_state = MAVLINK_PARSE_STATE_IDLE; |
| bhepp | 0:28183cc7963f | 525 | if (c == MAVLINK_STX) |
| bhepp | 0:28183cc7963f | 526 | { |
| bhepp | 0:28183cc7963f | 527 | status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; |
| bhepp | 0:28183cc7963f | 528 | rxmsg->len = 0; |
| bhepp | 0:28183cc7963f | 529 | mavlink_start_checksum(rxmsg); |
| bhepp | 0:28183cc7963f | 530 | } |
| bhepp | 0:28183cc7963f | 531 | return 0; |
| bhepp | 0:28183cc7963f | 532 | } |
| bhepp | 0:28183cc7963f | 533 | return msg_received; |
| bhepp | 0:28183cc7963f | 534 | } |
| bhepp | 0:28183cc7963f | 535 | |
| bhepp | 0:28183cc7963f | 536 | /** |
| bhepp | 0:28183cc7963f | 537 | * @brief Put a bitfield of length 1-32 bit into the buffer |
| bhepp | 0:28183cc7963f | 538 | * |
| bhepp | 0:28183cc7963f | 539 | * @param b the value to add, will be encoded in the bitfield |
| bhepp | 0:28183cc7963f | 540 | * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. |
| bhepp | 0:28183cc7963f | 541 | * @param packet_index the position in the packet (the index of the first byte to use) |
| bhepp | 0:28183cc7963f | 542 | * @param bit_index the position in the byte (the index of the first bit to use) |
| bhepp | 0:28183cc7963f | 543 | * @param buffer packet buffer to write into |
| bhepp | 0:28183cc7963f | 544 | * @return new position of the last used byte in the buffer |
| bhepp | 0:28183cc7963f | 545 | */ |
| bhepp | 0:28183cc7963f | 546 | MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) |
| bhepp | 0:28183cc7963f | 547 | { |
| bhepp | 0:28183cc7963f | 548 | uint16_t bits_remain = bits; |
| bhepp | 0:28183cc7963f | 549 | // Transform number into network order |
| bhepp | 0:28183cc7963f | 550 | int32_t v; |
| bhepp | 0:28183cc7963f | 551 | uint8_t i_bit_index, i_byte_index, curr_bits_n; |
| bhepp | 0:28183cc7963f | 552 | #if MAVLINK_NEED_BYTE_SWAP |
| bhepp | 0:28183cc7963f | 553 | union { |
| bhepp | 0:28183cc7963f | 554 | int32_t i; |
| bhepp | 0:28183cc7963f | 555 | uint8_t b[4]; |
| bhepp | 0:28183cc7963f | 556 | } bin, bout; |
| bhepp | 0:28183cc7963f | 557 | bin.i = b; |
| bhepp | 0:28183cc7963f | 558 | bout.b[0] = bin.b[3]; |
| bhepp | 0:28183cc7963f | 559 | bout.b[1] = bin.b[2]; |
| bhepp | 0:28183cc7963f | 560 | bout.b[2] = bin.b[1]; |
| bhepp | 0:28183cc7963f | 561 | bout.b[3] = bin.b[0]; |
| bhepp | 0:28183cc7963f | 562 | v = bout.i; |
| bhepp | 0:28183cc7963f | 563 | #else |
| bhepp | 0:28183cc7963f | 564 | v = b; |
| bhepp | 0:28183cc7963f | 565 | #endif |
| bhepp | 0:28183cc7963f | 566 | |
| bhepp | 0:28183cc7963f | 567 | // buffer in |
| bhepp | 0:28183cc7963f | 568 | // 01100000 01000000 00000000 11110001 |
| bhepp | 0:28183cc7963f | 569 | // buffer out |
| bhepp | 0:28183cc7963f | 570 | // 11110001 00000000 01000000 01100000 |
| bhepp | 0:28183cc7963f | 571 | |
| bhepp | 0:28183cc7963f | 572 | // Existing partly filled byte (four free slots) |
| bhepp | 0:28183cc7963f | 573 | // 0111xxxx |
| bhepp | 0:28183cc7963f | 574 | |
| bhepp | 0:28183cc7963f | 575 | // Mask n free bits |
| bhepp | 0:28183cc7963f | 576 | // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 |
| bhepp | 0:28183cc7963f | 577 | // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 |
| bhepp | 0:28183cc7963f | 578 | |
| bhepp | 0:28183cc7963f | 579 | // Shift n bits into the right position |
| bhepp | 0:28183cc7963f | 580 | // out = in >> n; |
| bhepp | 0:28183cc7963f | 581 | |
| bhepp | 0:28183cc7963f | 582 | // Mask and shift bytes |
| bhepp | 0:28183cc7963f | 583 | i_bit_index = bit_index; |
| bhepp | 0:28183cc7963f | 584 | i_byte_index = packet_index; |
| bhepp | 0:28183cc7963f | 585 | if (bit_index > 0) |
| bhepp | 0:28183cc7963f | 586 | { |
| bhepp | 0:28183cc7963f | 587 | // If bits were available at start, they were available |
| bhepp | 0:28183cc7963f | 588 | // in the byte before the current index |
| bhepp | 0:28183cc7963f | 589 | i_byte_index--; |
| bhepp | 0:28183cc7963f | 590 | } |
| bhepp | 0:28183cc7963f | 591 | |
| bhepp | 0:28183cc7963f | 592 | // While bits have not been packed yet |
| bhepp | 0:28183cc7963f | 593 | while (bits_remain > 0) |
| bhepp | 0:28183cc7963f | 594 | { |
| bhepp | 0:28183cc7963f | 595 | // Bits still have to be packed |
| bhepp | 0:28183cc7963f | 596 | // there can be more than 8 bits, so |
| bhepp | 0:28183cc7963f | 597 | // we might have to pack them into more than one byte |
| bhepp | 0:28183cc7963f | 598 | |
| bhepp | 0:28183cc7963f | 599 | // First pack everything we can into the current 'open' byte |
| bhepp | 0:28183cc7963f | 600 | //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 |
| bhepp | 0:28183cc7963f | 601 | //FIXME |
| bhepp | 0:28183cc7963f | 602 | if (bits_remain <= (uint8_t)(8 - i_bit_index)) |
| bhepp | 0:28183cc7963f | 603 | { |
| bhepp | 0:28183cc7963f | 604 | // Enough space |
| bhepp | 0:28183cc7963f | 605 | curr_bits_n = (uint8_t)bits_remain; |
| bhepp | 0:28183cc7963f | 606 | } |
| bhepp | 0:28183cc7963f | 607 | else |
| bhepp | 0:28183cc7963f | 608 | { |
| bhepp | 0:28183cc7963f | 609 | curr_bits_n = (8 - i_bit_index); |
| bhepp | 0:28183cc7963f | 610 | } |
| bhepp | 0:28183cc7963f | 611 | |
| bhepp | 0:28183cc7963f | 612 | // Pack these n bits into the current byte |
| bhepp | 0:28183cc7963f | 613 | // Mask out whatever was at that position with ones (xxx11111) |
| bhepp | 0:28183cc7963f | 614 | buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); |
| bhepp | 0:28183cc7963f | 615 | // Put content to this position, by masking out the non-used part |
| bhepp | 0:28183cc7963f | 616 | buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); |
| bhepp | 0:28183cc7963f | 617 | |
| bhepp | 0:28183cc7963f | 618 | // Increment the bit index |
| bhepp | 0:28183cc7963f | 619 | i_bit_index += curr_bits_n; |
| bhepp | 0:28183cc7963f | 620 | |
| bhepp | 0:28183cc7963f | 621 | // Now proceed to the next byte, if necessary |
| bhepp | 0:28183cc7963f | 622 | bits_remain -= curr_bits_n; |
| bhepp | 0:28183cc7963f | 623 | if (bits_remain > 0) |
| bhepp | 0:28183cc7963f | 624 | { |
| bhepp | 0:28183cc7963f | 625 | // Offer another 8 bits / one byte |
| bhepp | 0:28183cc7963f | 626 | i_byte_index++; |
| bhepp | 0:28183cc7963f | 627 | i_bit_index = 0; |
| bhepp | 0:28183cc7963f | 628 | } |
| bhepp | 0:28183cc7963f | 629 | } |
| bhepp | 0:28183cc7963f | 630 | |
| bhepp | 0:28183cc7963f | 631 | *r_bit_index = i_bit_index; |
| bhepp | 0:28183cc7963f | 632 | // If a partly filled byte is present, mark this as consumed |
| bhepp | 0:28183cc7963f | 633 | if (i_bit_index != 7) i_byte_index++; |
| bhepp | 0:28183cc7963f | 634 | return i_byte_index - packet_index; |
| bhepp | 0:28183cc7963f | 635 | } |
| bhepp | 0:28183cc7963f | 636 | |
| bhepp | 0:28183cc7963f | 637 | #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS |
| bhepp | 0:28183cc7963f | 638 | |
| bhepp | 0:28183cc7963f | 639 | // To make MAVLink work on your MCU, define comm_send_ch() if you wish |
| bhepp | 0:28183cc7963f | 640 | // to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a |
| bhepp | 0:28183cc7963f | 641 | // whole packet at a time |
| bhepp | 0:28183cc7963f | 642 | |
| bhepp | 0:28183cc7963f | 643 | /* |
| bhepp | 0:28183cc7963f | 644 | |
| bhepp | 0:28183cc7963f | 645 | #include "mavlink_types.h" |
| bhepp | 0:28183cc7963f | 646 | |
| bhepp | 0:28183cc7963f | 647 | void comm_send_ch(mavlink_channel_t chan, uint8_t ch) |
| bhepp | 0:28183cc7963f | 648 | { |
| bhepp | 0:28183cc7963f | 649 | if (chan == MAVLINK_COMM_0) |
| bhepp | 0:28183cc7963f | 650 | { |
| bhepp | 0:28183cc7963f | 651 | uart0_transmit(ch); |
| bhepp | 0:28183cc7963f | 652 | } |
| bhepp | 0:28183cc7963f | 653 | if (chan == MAVLINK_COMM_1) |
| bhepp | 0:28183cc7963f | 654 | { |
| bhepp | 0:28183cc7963f | 655 | uart1_transmit(ch); |
| bhepp | 0:28183cc7963f | 656 | } |
| bhepp | 0:28183cc7963f | 657 | } |
| bhepp | 0:28183cc7963f | 658 | */ |
| bhepp | 0:28183cc7963f | 659 | |
| bhepp | 0:28183cc7963f | 660 | MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) |
| bhepp | 0:28183cc7963f | 661 | { |
| bhepp | 0:28183cc7963f | 662 | #ifdef MAVLINK_SEND_UART_BYTES |
| bhepp | 0:28183cc7963f | 663 | /* this is the more efficient approach, if the platform |
| bhepp | 0:28183cc7963f | 664 | defines it */ |
| bhepp | 0:28183cc7963f | 665 | MAVLINK_SEND_UART_BYTES(chan, (const uint8_t *)buf, len); |
| bhepp | 0:28183cc7963f | 666 | #else |
| bhepp | 0:28183cc7963f | 667 | /* fallback to one byte at a time */ |
| bhepp | 0:28183cc7963f | 668 | uint16_t i; |
| bhepp | 0:28183cc7963f | 669 | for (i = 0; i < len; i++) { |
| bhepp | 0:28183cc7963f | 670 | comm_send_ch(chan, (uint8_t)buf[i]); |
| bhepp | 0:28183cc7963f | 671 | } |
| bhepp | 0:28183cc7963f | 672 | #endif |
| bhepp | 0:28183cc7963f | 673 | } |
| bhepp | 0:28183cc7963f | 674 | #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS |
| bhepp | 0:28183cc7963f | 675 | |
| bhepp | 0:28183cc7963f | 676 | #endif /* _MAVLINK_HELPERS_H_ */ |