Michael Shimniok
Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
Embed:
(wiki syntax)
Show/hide line numbers
protocol.h
00001 #ifndef _MAVLINK_PROTOCOL_H_ 00002 #define _MAVLINK_PROTOCOL_H_ 00003 00004 #include "string.h" 00005 #include "checksum.h" 00006 00007 #include "mavlink_types.h" 00008 00009 00010 /** 00011 * @brief Initialize the communication stack 00012 * 00013 * This function has to be called before using commParseBuffer() to initialize the different status registers. 00014 * 00015 * @return Will initialize the different buffers and status registers. 00016 */ 00017 static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) 00018 { 00019 if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) 00020 { 00021 initStatus->ck_a = 0; 00022 initStatus->ck_b = 0; 00023 initStatus->msg_received = 0; 00024 initStatus->buffer_overrun = 0; 00025 initStatus->parse_error = 0; 00026 initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; 00027 initStatus->packet_idx = 0; 00028 initStatus->packet_rx_drop_count = 0; 00029 initStatus->packet_rx_success_count = 0; 00030 initStatus->current_rx_seq = 0; 00031 initStatus->current_tx_seq = 0; 00032 } 00033 } 00034 00035 static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) 00036 { 00037 static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; 00038 return &m_mavlink_status[chan]; 00039 } 00040 00041 /** 00042 * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel 00043 * 00044 * This function calculates the checksum and sets length and aircraft id correctly. 00045 * It assumes that the message id and the payload are already correctly set. 00046 * 00047 * @warning This function implicitely assumes the message is sent over channel zero. 00048 * if the message is sent over a different channel it will reach the receiver 00049 * without error, BUT the sequence number might be wrong due to the wrong 00050 * channel sequence counter. This will result is wrongly reported excessive 00051 * packet loss. Please use @see mavlink_{pack|encode}_headerless and then 00052 * @see mavlink_finalize_message_chan before sending for a correct channel 00053 * assignment. Please note that the mavlink_msg_xxx_pack and encode functions 00054 * assign channel zero as default and thus induce possible loss counter errors.\ 00055 * They have been left to ensure code compatibility. 00056 * 00057 * @see mavlink_finalize_message_chan 00058 * @param msg Message to finalize 00059 * @param system_id Id of the sending (this) system, 1-127 00060 * @param length Message length, usually just the counter incremented while packing the message 00061 */ 00062 static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) 00063 { 00064 // This code part is the same for all messages; 00065 uint16_t checksum; 00066 msg->len = length; 00067 msg->sysid = system_id; 00068 msg->compid = component_id; 00069 // One sequence number per component 00070 msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; 00071 mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; 00072 checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); 00073 msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte 00074 msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte 00075 00076 return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; 00077 } 00078 00079 /** 00080 * @brief Finalize a MAVLink message with channel assignment 00081 * 00082 * This function calculates the checksum and sets length and aircraft id correctly. 00083 * It assumes that the message id and the payload are already correctly set. This function 00084 * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack 00085 * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. 00086 * 00087 * @param msg Message to finalize 00088 * @param system_id Id of the sending (this) system, 1-127 00089 * @param length Message length, usually just the counter incremented while packing the message 00090 */ 00091 static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) 00092 { 00093 // This code part is the same for all messages; 00094 uint16_t checksum; 00095 msg->len = length; 00096 msg->sysid = system_id; 00097 msg->compid = component_id; 00098 // One sequence number per component 00099 msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; 00100 mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; 00101 checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); 00102 msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte 00103 msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte 00104 00105 return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; 00106 } 00107 00108 /** 00109 * @brief Pack a message to send it over a serial byte stream 00110 */ 00111 static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) 00112 { 00113 *(buffer+0) = MAVLINK_STX; ///< Start transmit 00114 memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload 00115 *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; 00116 *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; 00117 return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; 00118 //return 0; 00119 } 00120 00121 /** 00122 * @brief Get the required buffer size for this message 00123 */ 00124 static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) 00125 { 00126 return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; 00127 } 00128 00129 union checksum_ { 00130 uint16_t s; 00131 uint8_t c[2]; 00132 }; 00133 00134 union __mavlink_bitfield { 00135 uint8_t uint8; 00136 int8_t int8; 00137 uint16_t uint16; 00138 int16_t int16; 00139 uint32_t uint32; 00140 int32_t int32; 00141 }; 00142 00143 00144 static inline void mavlink_start_checksum(mavlink_message_t* msg) 00145 { 00146 union checksum_ ck; 00147 crc_init(&(ck.s)); 00148 msg->ck_a = ck.c[0]; 00149 msg->ck_b = ck.c[1]; 00150 } 00151 00152 static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) 00153 { 00154 union checksum_ ck; 00155 ck.c[0] = msg->ck_a; 00156 ck.c[1] = msg->ck_b; 00157 crc_accumulate(c, &(ck.s)); 00158 msg->ck_a = ck.c[0]; 00159 msg->ck_b = ck.c[1]; 00160 } 00161 00162 /** 00163 * This is a convenience function which handles the complete MAVLink parsing. 00164 * the function will parse one byte at a time and return the complete packet once 00165 * it could be successfully decoded. Checksum and other failures will be silently 00166 * ignored. 00167 * 00168 * @param chan ID of the current channel. This allows to parse different channels with this function. 00169 * a channel is not a physical message channel like a serial port, but a logic partition of 00170 * the communication streams in this case. COMM_NB is the limit for the number of channels 00171 * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows 00172 * @param c The char to barse 00173 * 00174 * @param returnMsg NULL if no message could be decoded, the message data else 00175 * @return 0 if no message could be decoded, 1 else 00176 * 00177 * A typical use scenario of this function call is: 00178 * 00179 * @code 00180 * #include <inttypes.h> // For fixed-width uint8_t type 00181 * 00182 * mavlink_message_t msg; 00183 * int chan = 0; 00184 * 00185 * 00186 * while(serial.bytesAvailable > 0) 00187 * { 00188 * uint8_t byte = serial.getNextByte(); 00189 * if (mavlink_parse_char(chan, byte, &msg)) 00190 * { 00191 * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); 00192 * } 00193 * } 00194 * 00195 * 00196 * @endcode 00197 */ 00198 static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) 00199 { 00200 static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; 00201 00202 // Initializes only once, values keep unchanged after first initialization 00203 mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); 00204 00205 mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message 00206 mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status 00207 int bufferIndex = 0; 00208 00209 status->msg_received = 0; 00210 00211 switch (status->parse_state) 00212 { 00213 case MAVLINK_PARSE_STATE_UNINIT: 00214 case MAVLINK_PARSE_STATE_IDLE: 00215 if (c == MAVLINK_STX) 00216 { 00217 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; 00218 mavlink_start_checksum(rxmsg); 00219 } 00220 break; 00221 00222 case MAVLINK_PARSE_STATE_GOT_STX: 00223 if (status->msg_received) 00224 { 00225 status->buffer_overrun++; 00226 status->parse_error++; 00227 status->msg_received = 0; 00228 status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00229 } 00230 else 00231 { 00232 // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 00233 rxmsg->len = c; 00234 status->packet_idx = 0; 00235 mavlink_update_checksum(rxmsg, c); 00236 status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; 00237 } 00238 break; 00239 00240 case MAVLINK_PARSE_STATE_GOT_LENGTH: 00241 rxmsg->seq = c; 00242 mavlink_update_checksum(rxmsg, c); 00243 status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; 00244 break; 00245 00246 case MAVLINK_PARSE_STATE_GOT_SEQ: 00247 rxmsg->sysid = c; 00248 mavlink_update_checksum(rxmsg, c); 00249 status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; 00250 break; 00251 00252 case MAVLINK_PARSE_STATE_GOT_SYSID: 00253 rxmsg->compid = c; 00254 mavlink_update_checksum(rxmsg, c); 00255 status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; 00256 break; 00257 00258 case MAVLINK_PARSE_STATE_GOT_COMPID: 00259 rxmsg->msgid = c; 00260 mavlink_update_checksum(rxmsg, c); 00261 if (rxmsg->len == 0) 00262 { 00263 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; 00264 } 00265 else 00266 { 00267 status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; 00268 } 00269 break; 00270 00271 case MAVLINK_PARSE_STATE_GOT_MSGID: 00272 rxmsg->payload[status->packet_idx++] = c; 00273 mavlink_update_checksum(rxmsg, c); 00274 if (status->packet_idx == rxmsg->len) 00275 { 00276 status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; 00277 } 00278 break; 00279 00280 case MAVLINK_PARSE_STATE_GOT_PAYLOAD: 00281 if (c != rxmsg->ck_a) 00282 { 00283 // Check first checksum byte 00284 status->parse_error++; 00285 status->msg_received = 0; 00286 status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00287 if (c == MAVLINK_STX) 00288 { 00289 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; 00290 mavlink_start_checksum(rxmsg); 00291 } 00292 } 00293 else 00294 { 00295 status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; 00296 } 00297 break; 00298 00299 case MAVLINK_PARSE_STATE_GOT_CRC1: 00300 if (c != rxmsg->ck_b) 00301 {// Check second checksum byte 00302 status->parse_error++; 00303 status->msg_received = 0; 00304 status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00305 if (c == MAVLINK_STX) 00306 { 00307 status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; 00308 mavlink_start_checksum(rxmsg); 00309 } 00310 } 00311 else 00312 { 00313 // Successfully got message 00314 status->msg_received = 1; 00315 status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00316 memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); 00317 } 00318 break; 00319 } 00320 00321 bufferIndex++; 00322 // If a message has been sucessfully decoded, check index 00323 if (status->msg_received == 1) 00324 { 00325 //while(status->current_seq != rxmsg->seq) 00326 //{ 00327 // status->packet_rx_drop_count++; 00328 // status->current_seq++; 00329 //} 00330 status->current_rx_seq = rxmsg->seq; 00331 // Initial condition: If no packet has been received so far, drop count is undefined 00332 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; 00333 // Count this packet as received 00334 status->packet_rx_success_count++; 00335 } 00336 00337 r_mavlink_status->current_rx_seq = status->current_rx_seq+1; 00338 r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; 00339 r_mavlink_status->packet_rx_drop_count = status->parse_error; 00340 status->parse_error = 0; 00341 return status->msg_received; 00342 } 00343 00344 00345 /** 00346 * This is a convenience function which handles the complete MAVLink parsing. 00347 * the function will parse one byte at a time and return the complete packet once 00348 * it could be successfully decoded. Checksum and other failures will be silently 00349 * ignored. 00350 * 00351 * @param chan ID of the current channel. This allows to parse different channels with this function. 00352 * a channel is not a physical message channel like a serial port, but a logic partition of 00353 * the communication streams in this case. COMM_NB is the limit for the number of channels 00354 * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows 00355 * @param c The char to barse 00356 * 00357 * @param returnMsg NULL if no message could be decoded, the message data else 00358 * @return 0 if no message could be decoded, 1 else 00359 * 00360 * A typical use scenario of this function call is: 00361 * 00362 * @code 00363 * #include <inttypes.h> // For fixed-width uint8_t type 00364 * 00365 * mavlink_message_t msg; 00366 * int chan = 0; 00367 * 00368 * 00369 * while(serial.bytesAvailable > 0) 00370 * { 00371 * uint8_t byte = serial.getNextByte(); 00372 * if (mavlink_parse_char(chan, byte, &msg)) 00373 * { 00374 * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); 00375 * } 00376 * } 00377 * 00378 * 00379 * @endcode 00380 */ 00381 00382 #define MAVLINK_PACKET_START_CANDIDATES 50 00383 /* 00384 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) 00385 { 00386 static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; 00387 static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2]; 00388 static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS]; 00389 static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; 00390 static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES]; 00391 static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS]; 00392 static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS]; 00393 00394 // Set a packet start candidate index if sign is start sign 00395 if (c == MAVLINK_STX) 00396 { 00397 m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; 00398 } 00399 00400 // Parse normally, if a CRC mismatch occurs retry with the next packet index 00401 } 00402 // static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; 00403 // static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; 00404 //// Initializes only once, values keep unchanged after first initialization 00405 // mavlink_parse_state_initialize(&m_mavlink_status[chan]); 00406 // 00407 //mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message 00408 //mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status 00409 //int bufferIndex = 0; 00410 // 00411 //status->msg_received = 0; 00412 // 00413 //switch (status->parse_state) 00414 //{ 00415 //case MAVLINK_PARSE_STATE_UNINIT: 00416 //case MAVLINK_PARSE_STATE_IDLE: 00417 // if (c == MAVLINK_STX) 00418 // { 00419 // status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; 00420 // mavlink_start_checksum(rxmsg); 00421 // } 00422 // break; 00423 // 00424 //case MAVLINK_PARSE_STATE_GOT_STX: 00425 // if (status->msg_received) 00426 // { 00427 // status->buffer_overrun++; 00428 // status->parse_error++; 00429 // status->msg_received = 0; 00430 // status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00431 // } 00432 // else 00433 // { 00434 // // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 00435 // rxmsg->len = c; 00436 // status->packet_idx = 0; 00437 // mavlink_update_checksum(rxmsg, c); 00438 // status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; 00439 // } 00440 // break; 00441 // 00442 //case MAVLINK_PARSE_STATE_GOT_LENGTH: 00443 // rxmsg->seq = c; 00444 // mavlink_update_checksum(rxmsg, c); 00445 // status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; 00446 // break; 00447 // 00448 //case MAVLINK_PARSE_STATE_GOT_SEQ: 00449 // rxmsg->sysid = c; 00450 // mavlink_update_checksum(rxmsg, c); 00451 // status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; 00452 // break; 00453 // 00454 //case MAVLINK_PARSE_STATE_GOT_SYSID: 00455 // rxmsg->compid = c; 00456 // mavlink_update_checksum(rxmsg, c); 00457 // status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; 00458 // break; 00459 // 00460 //case MAVLINK_PARSE_STATE_GOT_COMPID: 00461 // rxmsg->msgid = c; 00462 // mavlink_update_checksum(rxmsg, c); 00463 // if (rxmsg->len == 0) 00464 // { 00465 // status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; 00466 // } 00467 // else 00468 // { 00469 // status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; 00470 // } 00471 // break; 00472 // 00473 //case MAVLINK_PARSE_STATE_GOT_MSGID: 00474 // rxmsg->payload[status->packet_idx++] = c; 00475 // mavlink_update_checksum(rxmsg, c); 00476 // if (status->packet_idx == rxmsg->len) 00477 // { 00478 // status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; 00479 // } 00480 // break; 00481 // 00482 //case MAVLINK_PARSE_STATE_GOT_PAYLOAD: 00483 // if (c != rxmsg->ck_a) 00484 // { 00485 // // Check first checksum byte 00486 // status->parse_error++; 00487 // status->msg_received = 0; 00488 // status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00489 // } 00490 // else 00491 // { 00492 // status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; 00493 // } 00494 // break; 00495 // 00496 //case MAVLINK_PARSE_STATE_GOT_CRC1: 00497 // if (c != rxmsg->ck_b) 00498 // {// Check second checksum byte 00499 // status->parse_error++; 00500 // status->msg_received = 0; 00501 // status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00502 // } 00503 // else 00504 // { 00505 // // Successfully got message 00506 // status->msg_received = 1; 00507 // status->parse_state = MAVLINK_PARSE_STATE_IDLE; 00508 // memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); 00509 // } 00510 // break; 00511 //} 00512 00513 bufferIndex++; 00514 // If a message has been sucessfully decoded, check index 00515 if (status->msg_received == 1) 00516 { 00517 //while(status->current_seq != rxmsg->seq) 00518 //{ 00519 // status->packet_rx_drop_count++; 00520 // status->current_seq++; 00521 //} 00522 status->current_seq = rxmsg->seq; 00523 // Initial condition: If no packet has been received so far, drop count is undefined 00524 if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; 00525 // Count this packet as received 00526 status->packet_rx_success_count++; 00527 } 00528 00529 r_mavlink_status->current_seq = status->current_seq+1; 00530 r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; 00531 r_mavlink_status->packet_rx_drop_count = status->parse_error; 00532 return status->msg_received; 00533 } 00534 */ 00535 00536 00537 typedef union __generic_16bit 00538 { 00539 uint8_t b[2]; 00540 int16_t s; 00541 } generic_16bit; 00542 00543 typedef union __generic_32bit 00544 { 00545 uint8_t b[4]; 00546 float f; 00547 int32_t i; 00548 int16_t s; 00549 } generic_32bit; 00550 00551 typedef union __generic_64bit 00552 { 00553 uint8_t b[8]; 00554 int64_t ll; ///< Long long (64 bit) 00555 double d; ///< IEEE-754 double precision floating point 00556 } generic_64bit; 00557 00558 /** 00559 * @brief Place an unsigned byte into the buffer 00560 * 00561 * @param b the byte to add 00562 * @param bindex the position in the packet 00563 * @param buffer the packet buffer 00564 * @return the new position of the last used byte in the buffer 00565 */ 00566 static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) 00567 { 00568 *(buffer + bindex) = b; 00569 return sizeof(b); 00570 } 00571 00572 /** 00573 * @brief Place a signed byte into the buffer 00574 * 00575 * @param b the byte to add 00576 * @param bindex the position in the packet 00577 * @param buffer the packet buffer 00578 * @return the new position of the last used byte in the buffer 00579 */ 00580 static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) 00581 { 00582 *(buffer + bindex) = (uint8_t)b; 00583 return sizeof(b); 00584 } 00585 00586 /** 00587 * @brief Place two unsigned bytes into the buffer 00588 * 00589 * @param b the bytes to add 00590 * @param bindex the position in the packet 00591 * @param buffer the packet buffer 00592 * @return the new position of the last used byte in the buffer 00593 */ 00594 static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) 00595 { 00596 buffer[bindex] = (b>>8)&0xff; 00597 buffer[bindex+1] = (b & 0xff); 00598 return sizeof(b); 00599 } 00600 00601 /** 00602 * @brief Place two signed bytes into the buffer 00603 * 00604 * @param b the bytes to add 00605 * @param bindex the position in the packet 00606 * @param buffer the packet buffer 00607 * @return the new position of the last used byte in the buffer 00608 */ 00609 static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) 00610 { 00611 return put_uint16_t_by_index(b, bindex, buffer); 00612 } 00613 00614 /** 00615 * @brief Place four unsigned bytes into the buffer 00616 * 00617 * @param b the bytes to add 00618 * @param bindex the position in the packet 00619 * @param buffer the packet buffer 00620 * @return the new position of the last used byte in the buffer 00621 */ 00622 static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) 00623 { 00624 buffer[bindex] = (b>>24)&0xff; 00625 buffer[bindex+1] = (b>>16)&0xff; 00626 buffer[bindex+2] = (b>>8)&0xff; 00627 buffer[bindex+3] = (b & 0xff); 00628 return sizeof(b); 00629 } 00630 00631 /** 00632 * @brief Place four signed bytes into the buffer 00633 * 00634 * @param b the bytes to add 00635 * @param bindex the position in the packet 00636 * @param buffer the packet buffer 00637 * @return the new position of the last used byte in the buffer 00638 */ 00639 static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) 00640 { 00641 buffer[bindex] = (b>>24)&0xff; 00642 buffer[bindex+1] = (b>>16)&0xff; 00643 buffer[bindex+2] = (b>>8)&0xff; 00644 buffer[bindex+3] = (b & 0xff); 00645 return sizeof(b); 00646 } 00647 00648 /** 00649 * @brief Place four unsigned bytes into the buffer 00650 * 00651 * @param b the bytes to add 00652 * @param bindex the position in the packet 00653 * @param buffer the packet buffer 00654 * @return the new position of the last used byte in the buffer 00655 */ 00656 static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) 00657 { 00658 buffer[bindex] = (b>>56)&0xff; 00659 buffer[bindex+1] = (b>>48)&0xff; 00660 buffer[bindex+2] = (b>>40)&0xff; 00661 buffer[bindex+3] = (b>>32)&0xff; 00662 buffer[bindex+4] = (b>>24)&0xff; 00663 buffer[bindex+5] = (b>>16)&0xff; 00664 buffer[bindex+6] = (b>>8)&0xff; 00665 buffer[bindex+7] = (b & 0xff); 00666 return sizeof(b); 00667 } 00668 00669 /** 00670 * @brief Place four signed bytes into the buffer 00671 * 00672 * @param b the bytes to add 00673 * @param bindex the position in the packet 00674 * @param buffer the packet buffer 00675 * @return the new position of the last used byte in the buffer 00676 */ 00677 static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) 00678 { 00679 return put_uint64_t_by_index(b, bindex, buffer); 00680 } 00681 00682 /** 00683 * @brief Place a float into the buffer 00684 * 00685 * @param b the float to add 00686 * @param bindex the position in the packet 00687 * @param buffer the packet buffer 00688 * @return the new position of the last used byte in the buffer 00689 */ 00690 static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) 00691 { 00692 generic_32bit g; 00693 g.f = b; 00694 return put_int32_t_by_index(g.i, bindex, buffer); 00695 } 00696 00697 /** 00698 * @brief Place a double into the buffer 00699 * 00700 * @param b the double to add 00701 * @param bindex the position in the packet 00702 * @param buffer the packet buffer 00703 * @return the new position of the last used byte in the buffer 00704 */ 00705 static inline uint8_t put_double_by_index(double b, uint8_t bindex, uint8_t* buffer) 00706 { 00707 generic_64bit g; 00708 g.d = b; 00709 return put_int64_t_by_index(g.ll, bindex, buffer); 00710 } 00711 00712 /** 00713 * @brief Place an array into the buffer 00714 * 00715 * @param b the array to add 00716 * @param length size of the array (for strings: length WITH '\0' char) 00717 * @param bindex the position in the packet 00718 * @param buffer packet buffer 00719 * @return new position of the last used byte in the buffer 00720 */ 00721 static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) 00722 { 00723 memcpy(buffer+bindex, b, length); 00724 return length; 00725 } 00726 00727 /** 00728 * @brief Place a string into the buffer 00729 * 00730 * @param b the string to add 00731 * @param maxlength size of the array (for strings: length WITHOUT '\0' char) 00732 * @param bindex the position in the packet 00733 * @param buffer packet buffer 00734 * @return new position of the last used byte in the buffer 00735 */ 00736 static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) 00737 { 00738 uint16_t length = 0; 00739 // Copy string into buffer, ensuring not to exceed the buffer size 00740 int i; 00741 for (i = 1; i < maxlength; i++) 00742 { 00743 length++; 00744 // String characters 00745 if (i < (maxlength - 1)) 00746 { 00747 buffer[bindex+i] = b[i]; 00748 // Stop at null character 00749 if (b[i] == '\0') 00750 { 00751 break; 00752 } 00753 } 00754 // Enforce null termination at end of buffer 00755 else if (i == (maxlength - 1)) 00756 { 00757 buffer[i] = '\0'; 00758 } 00759 } 00760 // Write length into first field 00761 put_uint8_t_by_index(length, bindex, buffer); 00762 return length; 00763 } 00764 00765 /** 00766 * @brief Put a bitfield of length 1-32 bit into the buffer 00767 * 00768 * @param b the value to add, will be encoded in the bitfield 00769 * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. 00770 * @param packet_index the position in the packet (the index of the first byte to use) 00771 * @param bit_index the position in the byte (the index of the first bit to use) 00772 * @param buffer packet buffer to write into 00773 * @return new position of the last used byte in the buffer 00774 */ 00775 static inline 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) 00776 { 00777 uint16_t bits_remain = bits; 00778 // Transform number into network order 00779 generic_32bit bin; 00780 generic_32bit bout; 00781 uint8_t i_bit_index, i_byte_index, curr_bits_n; 00782 bin.i = b; 00783 bout.b[0] = bin.b[3]; 00784 bout.b[1] = bin.b[2]; 00785 bout.b[2] = bin.b[1]; 00786 bout.b[3] = bin.b[0]; 00787 00788 // buffer in 00789 // 01100000 01000000 00000000 11110001 00790 // buffer out 00791 // 11110001 00000000 01000000 01100000 00792 00793 // Existing partly filled byte (four free slots) 00794 // 0111xxxx 00795 00796 // Mask n free bits 00797 // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 00798 // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 00799 00800 // Shift n bits into the right position 00801 // out = in >> n; 00802 00803 // Mask and shift bytes 00804 i_bit_index = bit_index; 00805 i_byte_index = packet_index; 00806 if (bit_index > 0) 00807 { 00808 // If bits were available at start, they were available 00809 // in the byte before the current index 00810 i_byte_index--; 00811 } 00812 00813 // While bits have not been packed yet 00814 while (bits_remain > 0) 00815 { 00816 // Bits still have to be packed 00817 // there can be more than 8 bits, so 00818 // we might have to pack them into more than one byte 00819 00820 // First pack everything we can into the current 'open' byte 00821 //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 00822 //FIXME 00823 if (bits_remain <= (8 - i_bit_index)) 00824 { 00825 // Enough space 00826 curr_bits_n = bits_remain; 00827 } 00828 else 00829 { 00830 curr_bits_n = (8 - i_bit_index); 00831 } 00832 00833 // Pack these n bits into the current byte 00834 // Mask out whatever was at that position with ones (xxx11111) 00835 buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); 00836 // Put content to this position, by masking out the non-used part 00837 buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); 00838 00839 // Increment the bit index 00840 i_bit_index += curr_bits_n; 00841 00842 // Now proceed to the next byte, if necessary 00843 bits_remain -= curr_bits_n; 00844 if (bits_remain > 0) 00845 { 00846 // Offer another 8 bits / one byte 00847 i_byte_index++; 00848 i_bit_index = 0; 00849 } 00850 } 00851 00852 *r_bit_index = i_bit_index; 00853 // If a partly filled byte is present, mark this as consumed 00854 if (i_bit_index != 7) i_byte_index++; 00855 return i_byte_index - packet_index; 00856 } 00857 00858 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 00859 00860 // To make MAVLink work on your MCU, define a similar function 00861 00862 /* 00863 00864 #include "mavlink_types.h" 00865 00866 void comm_send_ch(mavlink_channel_t chan, uint8_t ch) 00867 { 00868 if (chan == MAVLINK_COMM_0) 00869 { 00870 uart0_transmit(ch); 00871 } 00872 if (chan == MAVLINK_COMM_1) 00873 { 00874 uart1_transmit(ch); 00875 } 00876 } 00877 */ 00878 00879 static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum) 00880 { 00881 crc_accumulate(b, checksum); 00882 comm_send_ch(chan, b); 00883 } 00884 00885 static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum) 00886 { 00887 crc_accumulate(b, checksum); 00888 comm_send_ch(chan, b); 00889 } 00890 00891 static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum) 00892 { 00893 char s; 00894 s = (b>>8)&0xff; 00895 comm_send_ch(chan, s); 00896 crc_accumulate(s, checksum); 00897 s = (b & 0xff); 00898 comm_send_ch(chan, s); 00899 crc_accumulate(s, checksum); 00900 } 00901 00902 static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum) 00903 { 00904 mavlink_send_uart_uint16_t(chan, b, checksum); 00905 } 00906 00907 static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum) 00908 { 00909 char s; 00910 s = (b>>24)&0xff; 00911 comm_send_ch(chan, s); 00912 crc_accumulate(s, checksum); 00913 s = (b>>16)&0xff; 00914 comm_send_ch(chan, s); 00915 crc_accumulate(s, checksum); 00916 s = (b>>8)&0xff; 00917 comm_send_ch(chan, s); 00918 crc_accumulate(s, checksum); 00919 s = (b & 0xff); 00920 comm_send_ch(chan, s); 00921 crc_accumulate(s, checksum); 00922 } 00923 00924 static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum) 00925 { 00926 mavlink_send_uart_uint32_t(chan, b, checksum); 00927 } 00928 00929 static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum) 00930 { 00931 char s; 00932 s = (b>>56)&0xff; 00933 comm_send_ch(chan, s); 00934 crc_accumulate(s, checksum); 00935 s = (b>>48)&0xff; 00936 comm_send_ch(chan, s); 00937 crc_accumulate(s, checksum); 00938 s = (b>>40)&0xff; 00939 comm_send_ch(chan, s); 00940 crc_accumulate(s, checksum); 00941 s = (b>>32)&0xff; 00942 comm_send_ch(chan, s); 00943 crc_accumulate(s, checksum); 00944 s = (b>>24)&0xff; 00945 comm_send_ch(chan, s); 00946 crc_accumulate(s, checksum); 00947 s = (b>>16)&0xff; 00948 comm_send_ch(chan, s); 00949 crc_accumulate(s, checksum); 00950 s = (b>>8)&0xff; 00951 comm_send_ch(chan, s); 00952 crc_accumulate(s, checksum); 00953 s = (b & 0xff); 00954 comm_send_ch(chan, s); 00955 crc_accumulate(s, checksum); 00956 } 00957 00958 static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum) 00959 { 00960 mavlink_send_uart_uint64_t(chan, b, checksum); 00961 } 00962 00963 static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum) 00964 { 00965 generic_32bit g; 00966 g.f = b; 00967 mavlink_send_uart_uint32_t(chan, g.i, checksum); 00968 } 00969 00970 static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum) 00971 { 00972 generic_64bit g; 00973 g.d = b; 00974 mavlink_send_uart_uint32_t(chan, g.ll, checksum); 00975 } 00976 00977 static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) 00978 { 00979 // ARM7 MCU board implementation 00980 // Create pointer on message struct 00981 // Send STX 00982 comm_send_ch(chan, MAVLINK_STX); 00983 comm_send_ch(chan, msg->len); 00984 comm_send_ch(chan, msg->seq); 00985 comm_send_ch(chan, msg->sysid); 00986 comm_send_ch(chan, msg->compid); 00987 comm_send_ch(chan, msg->msgid); 00988 for(uint16_t i = 0; i < msg->len; i++) 00989 { 00990 comm_send_ch(chan, msg->payload[i]); 00991 } 00992 comm_send_ch(chan, msg->ck_a); 00993 comm_send_ch(chan, msg->ck_b); 00994 } 00995 #endif 00996 00997 #endif /* _MAVLINK_PROTOCOL_H_ */
Generated on Tue Jul 12 2022 14:09:27 by
1.7.2