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 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;
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
{
// This code part is the same for all messages;
uint16_t checksum;
msg->magic = MAVLINK_STX;
msg->len = length;
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
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);
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
crc_accumulate(crc_extra, &checksum);
crc_accumulate(crc_extra, &msg->checksum);
#endif
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
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[5] = msgid;
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);
#if MAVLINK_CRC_EXTRA
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[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_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)
{
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;
}

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