aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

MavLink Update

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1795 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
mich146@hotmail.com 2011-03-20 23:28:42 +00:00
parent ed202447e6
commit 5008c1656b
3 changed files with 40 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/GCS_MAVLink/include/common/common.h
View File

@ -102,7 +102,6 @@ enum MAV_DATA_STREAM
#include "./mavlink_msg_raw_imu.h"
#include "./mavlink_msg_raw_pressure.h"
#include "./mavlink_msg_attitude.h"
#include "./mavlink_msg_attitude_new.h"
#include "./mavlink_msg_local_position.h"
#include "./mavlink_msg_global_position.h"
#include "./mavlink_msg_gps_raw.h"

19
libraries/GCS_MAVLink/include/mavlink_types.h
View File

@ -179,21 +179,10 @@ typedef enum {
MAVLINK_COMM_0,
MAVLINK_COMM_1,
MAVLINK_COMM_2,
MAVLINK_COMM_3
} mavlink_channel_t;
/*
* applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
* of buffers they will use. If more are used, then the result will be
* a stack overrun
*/
#ifndef MAVLINK_COMM_NUM_BUFFERS
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
# define MAVLINK_COMM_NUM_BUFFERS 16
#else
# define MAVLINK_COMM_NUM_BUFFERS 4
#endif
#endif
MAVLINK_COMM_3,
MAVLINK_COMM_NB,
MAVLINK_COMM_NB_HIGH = 16
} mavlink_channel_t;
typedef enum {
MAVLINK_PARSE_STATE_UNINIT=0,

147
libraries/GCS_MAVLink/include/protocol.h
View File

@ -34,7 +34,12 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus)
static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
#else
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
#endif
return &m_mavlink_status[chan];
}
@ -68,7 +73,7 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t
msg->compid = component_id;
// One sequence number per component
msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq;
mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
@ -197,8 +202,11 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
*/
static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
#else
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
#endif
// Initializes only once, values keep unchanged after first initialization
mavlink_parse_state_initialize(mavlink_get_channel_status(chan));
@ -383,13 +391,23 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag
/*
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)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2];
static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS];
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES];
static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS];
static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS];
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2];
static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH];
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES];
static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH];
static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH];
#else
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2];
static uint8_t m_msgbuf_index[MAVLINK_COMM_NB];
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES];
static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB];
static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB];
#endif
// Set a packet start candidate index if sign is start sign
if (c == MAVLINK_STX)
@ -399,8 +417,13 @@ static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_me
// Parse normally, if a CRC mismatch occurs retry with the next packet index
}
// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
//#else
// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
//#endif
//// Initializes only once, values keep unchanged after first initialization
// mavlink_parse_state_initialize(&m_mavlink_status[chan]);
//
@ -552,7 +575,6 @@ typedef union __generic_64bit
{
uint8_t b[8];
int64_t ll; ///< Long long (64 bit)
double d; ///< IEEE-754 double precision floating point
} generic_64bit;
/**
@ -861,103 +883,6 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
}
*/
static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum)
{
crc_accumulate(b, checksum);
comm_send_ch(chan, b);
}
static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum)
{
crc_accumulate(b, checksum);
comm_send_ch(chan, b);
}
static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum)
{
char s;
s = (b>>8)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b & 0xff);
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
}
static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum)
{
mavlink_send_uart_uint16_t(chan, b, checksum);
}
static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum)
{
char s;
s = (b>>24)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>16)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>8)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b & 0xff);
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
}
static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum)
{
mavlink_send_uart_uint32_t(chan, b, checksum);
}
static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum)
{
char s;
s = (b>>56)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>48)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>40)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>32)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>24)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>16)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b>>8)&0xff;
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
s = (b & 0xff);
comm_send_ch(chan, s);
crc_accumulate(s, checksum);
}
static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum)
{
mavlink_send_uart_uint64_t(chan, b, checksum);
}
static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum)
{
generic_32bit g;
g.f = b;
mavlink_send_uart_uint32_t(chan, g.i, checksum);
}
static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum)
{
generic_64bit g;
g.d = b;
mavlink_send_uart_uint32_t(chan, g.ll, checksum);
}
static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg)
{