Emaxx Navigation Group / mavlink_emaxx

Mavlink Messages for Emaxx Nav Board

Dependents:   Madpulse_Speed_Control_temp Madpulse_Speed_Control_Students

mavlink_helpers.h@1:7e9af9a921f7, 2017-01-20 (annotated)

Committer:
jdawkins
Date:
Fri Jan 20 13:27:11 2017 +0000
Revision:
1:7e9af9a921f7
Parent:
0:bb2cacd02294 
Fixed Asin error in mavlink_conversions.h

Who changed what in which revision?

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