diff options
Diffstat (limited to 'mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h')
| -rw-r--r-- | mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h | 507 |
1 files changed, 0 insertions, 507 deletions
diff --git a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h b/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h deleted file mode 100644 index 39d6930e5..000000000 --- a/mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h +++ /dev/null @@ -1,507 +0,0 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ - -#include "string.h" -#include "checksum.h" -#include "mavlink_types.h" - -#ifndef MAVLINK_HELPER -#define MAVLINK_HELPER -#endif - -/* - internal function to give access to the channel status for each channel - */ -MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; -} - -/* - internal function to give access to the channel buffer for each channel - */ -MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) -{ - -#if MAVLINK_EXTERNAL_RX_BUFFER - // No m_mavlink_message array defined in function, - // has to be defined externally -#ifndef m_mavlink_message -#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];) -#endif -#else - static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS]; -#endif - return &m_mavlink_buffer[chan]; -} - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) -#endif -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->magic = MAVLINK_STX; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); - - return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length, uint8_t crc_extra) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); -} -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) -{ - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); - -/** - * @brief Finalize a MAVLink message with channel assignment and send - */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, - uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) -#endif -{ - uint16_t checksum; - uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; - uint8_t ck[2]; - mavlink_status_t *status = mavlink_get_channel_status(chan); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid; - status->current_tx_seq++; - checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&checksum, packet, length); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &checksum); -#endif - ck[0] = (uint8_t)(checksum & 0xFF); - ck[1] = (uint8_t)(checksum >> 8); - - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); - _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); - _mavlink_send_uart(chan, packet, length); - _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a message to send it over a serial byte stream - */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) -{ - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); - return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; -} - -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - -MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) -{ - crc_init(&msg->checksum); -} - -MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - crc_accumulate(c, &msg->checksum); -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include <inttypes.h> // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -{ - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ -#if MAVLINK_CRC_EXTRA -#ifndef MAVLINK_MESSAGE_CRC - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] -#endif -#endif - - mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - rxmsg->magic = c; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received -/* Support shorter buffers than the - default maximum packet size */ -#if (MAVLINK_MAX_PAYLOAD_LEN < 255) - || c > MAVLINK_MAX_PAYLOAD_LEN -#endif - ) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -#if MAVLINK_CRC_EXTRA - mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); -#endif - if (c != (rxmsg->checksum & 0xFF)) { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != (rxmsg->checksum >> 8)) { - // Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - rxmsg->len = 0; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - return status->msg_received; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -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) -{ - uint16_t bits_remain = bits; - // Transform number into network order - int32_t v; - uint8_t i_bit_index, i_byte_index, curr_bits_n; -#if MAVLINK_NEED_BYTE_SWAP - union { - int32_t i; - uint8_t b[4]; - } bin, bout; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - v = bout.i; -#else - v = b; -#endif - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (uint8_t)(8 - i_bit_index)) - { - // Enough space - curr_bits_n = (uint8_t)bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define comm_send_ch() if you wish -// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a -// whole packet at a time - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) -{ -#ifdef MAVLINK_SEND_UART_BYTES - /* this is the more efficient approach, if the platform - defines it */ - MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len); -#else - /* fallback to one byte at a time */ - uint16_t i; - for (i = 0; i < len; i++) { - comm_send_ch(chan, (uint8_t)buf[i]); - } -#endif -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - -#endif /* _MAVLINK_HELPERS_H_ */ |