dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

MAVLink helper function fixes

This commit is contained in:
Lorenz Meier 2014-06-08 18:50:36 +02:00
parent 4b70a0d046
commit dfe71b615c
3 changed files with 17 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mavlink/include/mavlink/v1.0/checksum.h
View File

@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
* @param data new bytes to hash * @param data new bytes to hash
* @param crcAccum the already accumulated checksum * @param crcAccum the already accumulated checksum
**/ **/
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length)
{ {
const uint8_t *p = (const uint8_t *)pBuffer; const uint8_t *p = (const uint8_t *)pBuffer;
while (length--) { while (length--) {

21
mavlink/include/mavlink/v1.0/mavlink_helpers.h
View File

@ -73,7 +73,6 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
#endif #endif
{ {
// This code part is the same for all messages; // This code part is the same for all messages;
uint16_t checksum;
msg->magic = MAVLINK_STX; msg->magic = MAVLINK_STX;
msg->len = length; msg->len = length;
msg->sysid = system_id; msg->sysid = system_id;
@ -81,12 +80,13 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
// One sequence number per component // One sequence number per component
msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; 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; 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); msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum); crc_accumulate(crc_extra, &msg->checksum);
#endif #endif
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
} }
@ -133,7 +133,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
buf[4] = mavlink_system.compid; buf[4] = mavlink_system.compid;
buf[5] = msgid; buf[5] = msgid;
status->current_tx_seq++; status->current_tx_seq++;
checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&checksum, packet, length); crc_accumulate_buffer(&checksum, packet, length);
#if MAVLINK_CRC_EXTRA #if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum); crc_accumulate(crc_extra, &checksum);
@ -158,6 +158,7 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8); ck[1] = (uint8_t)(msg->checksum >> 8);
// XXX use the right sequence here
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
@ -172,7 +173,13 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
*/ */
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) 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); memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
ck[0] = (uint8_t)(msg->checksum & 0xFF);
ck[1] = (uint8_t)(msg->checksum >> 8);
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
} }

4
mavlink/include/mavlink/v1.0/mavlink_types.h
View File

@ -28,6 +28,7 @@
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) #define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
#pragma pack(push, 1)
typedef struct param_union { typedef struct param_union {
union { union {
float param_float; float param_float;
@ -62,13 +63,12 @@ typedef struct __mavlink_message {
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t; } mavlink_message_t;
typedef struct __mavlink_extended_message { typedef struct __mavlink_extended_message {
mavlink_message_t base_msg; mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t; } mavlink_extended_message_t;
#pragma pack(pop)
typedef enum { typedef enum {
MAVLINK_TYPE_CHAR = 0, MAVLINK_TYPE_CHAR = 0,