mirror of https://github.com/ArduPilot/ardupilot
mavlink: sync with upstream
this syncs with 7e1c274437f09a8a03a5bef990e14b93c8fc8b07 from March 17 2011 git-svn-id: https://arducopter.googlecode.com/svn/trunk@1782 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
cc548e4c19
commit
f5b1c84e2d
|
@ -102,6 +102,7 @@ enum MAV_DATA_STREAM
|
||||||
#include "./mavlink_msg_raw_imu.h"
|
#include "./mavlink_msg_raw_imu.h"
|
||||||
#include "./mavlink_msg_raw_pressure.h"
|
#include "./mavlink_msg_raw_pressure.h"
|
||||||
#include "./mavlink_msg_attitude.h"
|
#include "./mavlink_msg_attitude.h"
|
||||||
|
#include "./mavlink_msg_attitude_new.h"
|
||||||
#include "./mavlink_msg_local_position.h"
|
#include "./mavlink_msg_local_position.h"
|
||||||
#include "./mavlink_msg_global_position.h"
|
#include "./mavlink_msg_global_position.h"
|
||||||
#include "./mavlink_msg_gps_raw.h"
|
#include "./mavlink_msg_gps_raw.h"
|
||||||
|
|
|
@ -0,0 +1,268 @@
|
||||||
|
// MESSAGE ATTITUDE PACKING
|
||||||
|
|
||||||
|
#define MAVLINK_MSG_ID_ATTITUDE_NEW 30
|
||||||
|
#define MAVLINK_MSG_LENGTH_ATTITUDE_NEW 8+4+4+4+4+4+4
|
||||||
|
|
||||||
|
#ifndef MAVLINK_DEACTIVATE_STRUCTS
|
||||||
|
|
||||||
|
typedef struct __mavlink_attitude_new_t
|
||||||
|
{
|
||||||
|
uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
float roll; ///< Roll angle (rad)
|
||||||
|
float pitch; ///< Pitch angle (rad)
|
||||||
|
float yaw; ///< Yaw angle (rad)
|
||||||
|
float rollspeed; ///< Roll angular speed (rad/s)
|
||||||
|
float pitchspeed; ///< Pitch angular speed (rad/s)
|
||||||
|
float yawspeed; ///< Yaw angular speed (rad/s)
|
||||||
|
|
||||||
|
} mavlink_attitude_new_t;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Pack a attitude_new message
|
||||||
|
* @param system_id ID of this system
|
||||||
|
* @param component_id ID of this component (e.g. 200 for IMU)
|
||||||
|
* @param msg The MAVLink message to compress the data into
|
||||||
|
*
|
||||||
|
* @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
* @param roll Roll angle (rad)
|
||||||
|
* @param pitch Pitch angle (rad)
|
||||||
|
* @param yaw Yaw angle (rad)
|
||||||
|
* @param rollspeed Roll angular speed (rad/s)
|
||||||
|
* @param pitchspeed Pitch angular speed (rad/s)
|
||||||
|
* @param yawspeed Yaw angular speed (rad/s)
|
||||||
|
* @return length of the message in bytes (excluding serial stream start sign)
|
||||||
|
*/
|
||||||
|
static inline uint16_t mavlink_msg_attitude_new_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
|
||||||
|
{
|
||||||
|
uint16_t i = 0;
|
||||||
|
msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
|
||||||
|
|
||||||
|
i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
|
||||||
|
i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
|
||||||
|
i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
|
||||||
|
i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
|
||||||
|
i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
|
||||||
|
i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
|
||||||
|
|
||||||
|
return mavlink_finalize_message(msg, system_id, component_id, i);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Pack a attitude_new message
|
||||||
|
* @param system_id ID of this system
|
||||||
|
* @param component_id ID of this component (e.g. 200 for IMU)
|
||||||
|
* @param chan The MAVLink channel this message was sent over
|
||||||
|
* @param msg The MAVLink message to compress the data into
|
||||||
|
* @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
* @param roll Roll angle (rad)
|
||||||
|
* @param pitch Pitch angle (rad)
|
||||||
|
* @param yaw Yaw angle (rad)
|
||||||
|
* @param rollspeed Roll angular speed (rad/s)
|
||||||
|
* @param pitchspeed Pitch angular speed (rad/s)
|
||||||
|
* @param yawspeed Yaw angular speed (rad/s)
|
||||||
|
* @return length of the message in bytes (excluding serial stream start sign)
|
||||||
|
*/
|
||||||
|
static inline uint16_t mavlink_msg_attitude_new_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
|
||||||
|
{
|
||||||
|
uint16_t i = 0;
|
||||||
|
msg->msgid = MAVLINK_MSG_ID_ATTITUDE;
|
||||||
|
|
||||||
|
i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
|
||||||
|
i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
|
||||||
|
i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
|
||||||
|
i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
|
||||||
|
i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
|
||||||
|
i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
|
||||||
|
|
||||||
|
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Encode a attitude_new struct into a message
|
||||||
|
*
|
||||||
|
* @param system_id ID of this system
|
||||||
|
* @param component_id ID of this component (e.g. 200 for IMU)
|
||||||
|
* @param msg The MAVLink message to compress the data into
|
||||||
|
* @param attitude_new C-struct to read the message contents from
|
||||||
|
*/
|
||||||
|
static inline uint16_t mavlink_msg_attitude_new_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_new_t* attitude_new)
|
||||||
|
{
|
||||||
|
return mavlink_msg_attitude_new_pack(system_id, component_id, msg, attitude_new->usec, attitude_new->roll, attitude_new->pitch, attitude_new->yaw, attitude_new->rollspeed, attitude_new->pitchspeed, attitude_new->yawspeed);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Send a attitude_new message
|
||||||
|
* @param chan MAVLink channel to send the message
|
||||||
|
*
|
||||||
|
* @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
* @param roll Roll angle (rad)
|
||||||
|
* @param pitch Pitch angle (rad)
|
||||||
|
* @param yaw Yaw angle (rad)
|
||||||
|
* @param rollspeed Roll angular speed (rad/s)
|
||||||
|
* @param pitchspeed Pitch angular speed (rad/s)
|
||||||
|
* @param yawspeed Yaw angular speed (rad/s)
|
||||||
|
*/
|
||||||
|
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||||
|
|
||||||
|
static inline void mavlink_msg_attitude_new_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
|
||||||
|
{
|
||||||
|
uint16_t checksum;
|
||||||
|
comm_send_ch(chan, MAVLINK_STX);
|
||||||
|
crc_init(&checksum);
|
||||||
|
mavlink_send_uart_uint8_t(chan, MAVLINK_MSG_LENGTH_ATTITUDE_NEW, &checksum);
|
||||||
|
mavlink_send_uart_uint8_t(chan, mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq, &checksum);
|
||||||
|
// Increase sequence
|
||||||
|
mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1;
|
||||||
|
mavlink_send_uart_uint8_t(chan, mavlink_system.sysid, &checksum);
|
||||||
|
mavlink_send_uart_uint8_t(chan, mavlink_system.compid, &checksum);
|
||||||
|
mavlink_send_uart_uint8_t(chan, MAVLINK_MSG_ID_ATTITUDE_NEW, &checksum);
|
||||||
|
mavlink_send_uart_uint64_t(chan, usec, &checksum);
|
||||||
|
mavlink_send_uart_float(chan, roll, &checksum);
|
||||||
|
mavlink_send_uart_float(chan, pitch, &checksum);
|
||||||
|
mavlink_send_uart_float(chan, yaw, &checksum);
|
||||||
|
mavlink_send_uart_float(chan, rollspeed, &checksum);
|
||||||
|
mavlink_send_uart_float(chan, pitchspeed, &checksum);
|
||||||
|
mavlink_send_uart_float(chan, yawspeed, &checksum);
|
||||||
|
// Checksum complete, send it
|
||||||
|
comm_send_ch(chan, (uint8_t)(checksum & 0xFF));
|
||||||
|
comm_send_ch(chan, (uint8_t)(checksum >> 8));
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
// MESSAGE ATTITUDE UNPACKING
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field usec from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||||
|
*/
|
||||||
|
static inline uint64_t mavlink_msg_attitude_new_get_usec(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_64bit r;
|
||||||
|
r.b[7] = (msg->payload)[0];
|
||||||
|
r.b[6] = (msg->payload)[1];
|
||||||
|
r.b[5] = (msg->payload)[2];
|
||||||
|
r.b[4] = (msg->payload)[3];
|
||||||
|
r.b[3] = (msg->payload)[4];
|
||||||
|
r.b[2] = (msg->payload)[5];
|
||||||
|
r.b[1] = (msg->payload)[6];
|
||||||
|
r.b[0] = (msg->payload)[7];
|
||||||
|
return (uint64_t)r.ll;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field roll from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Roll angle (rad)
|
||||||
|
*/
|
||||||
|
static inline float mavlink_msg_attitude_new_get_roll(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_32bit r;
|
||||||
|
r.b[3] = (msg->payload+sizeof(uint64_t))[0];
|
||||||
|
r.b[2] = (msg->payload+sizeof(uint64_t))[1];
|
||||||
|
r.b[1] = (msg->payload+sizeof(uint64_t))[2];
|
||||||
|
r.b[0] = (msg->payload+sizeof(uint64_t))[3];
|
||||||
|
return (float)r.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field pitch from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Pitch angle (rad)
|
||||||
|
*/
|
||||||
|
static inline float mavlink_msg_attitude_new_get_pitch(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_32bit r;
|
||||||
|
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
|
||||||
|
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
|
||||||
|
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
|
||||||
|
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
|
||||||
|
return (float)r.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field yaw from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Yaw angle (rad)
|
||||||
|
*/
|
||||||
|
static inline float mavlink_msg_attitude_new_get_yaw(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_32bit r;
|
||||||
|
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
|
||||||
|
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
|
||||||
|
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
|
||||||
|
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
|
||||||
|
return (float)r.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field rollspeed from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Roll angular speed (rad/s)
|
||||||
|
*/
|
||||||
|
static inline float mavlink_msg_attitude_new_get_rollspeed(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_32bit r;
|
||||||
|
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
|
||||||
|
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
|
||||||
|
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
|
||||||
|
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
|
||||||
|
return (float)r.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field pitchspeed from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Pitch angular speed (rad/s)
|
||||||
|
*/
|
||||||
|
static inline float mavlink_msg_attitude_new_get_pitchspeed(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_32bit r;
|
||||||
|
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
|
||||||
|
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
|
||||||
|
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
|
||||||
|
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
|
||||||
|
return (float)r.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Get field yawspeed from attitude_new message
|
||||||
|
*
|
||||||
|
* @return Yaw angular speed (rad/s)
|
||||||
|
*/
|
||||||
|
static inline float mavlink_msg_attitude_new_get_yawspeed(const mavlink_message_t* msg)
|
||||||
|
{
|
||||||
|
generic_32bit r;
|
||||||
|
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
|
||||||
|
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
|
||||||
|
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
|
||||||
|
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
|
||||||
|
return (float)r.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifndef MAVLINK_DEACTIVATE_STRUCTS
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Decode a attitude_new message into a struct
|
||||||
|
*
|
||||||
|
* @param msg The message to decode
|
||||||
|
* @param attitude_new C-struct to decode the message contents into
|
||||||
|
*/
|
||||||
|
static inline void mavlink_msg_attitude_new_decode(const mavlink_message_t* msg, mavlink_attitude_new_t* attitude_new)
|
||||||
|
{
|
||||||
|
attitude_new->usec = mavlink_msg_attitude_new_get_usec(msg);
|
||||||
|
attitude_new->roll = mavlink_msg_attitude_new_get_roll(msg);
|
||||||
|
attitude_new->pitch = mavlink_msg_attitude_new_get_pitch(msg);
|
||||||
|
attitude_new->yaw = mavlink_msg_attitude_new_get_yaw(msg);
|
||||||
|
attitude_new->rollspeed = mavlink_msg_attitude_new_get_rollspeed(msg);
|
||||||
|
attitude_new->pitchspeed = mavlink_msg_attitude_new_get_pitchspeed(msg);
|
||||||
|
attitude_new->yawspeed = mavlink_msg_attitude_new_get_yawspeed(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
|
@ -179,10 +179,21 @@ typedef enum {
|
||||||
MAVLINK_COMM_0,
|
MAVLINK_COMM_0,
|
||||||
MAVLINK_COMM_1,
|
MAVLINK_COMM_1,
|
||||||
MAVLINK_COMM_2,
|
MAVLINK_COMM_2,
|
||||||
MAVLINK_COMM_3,
|
MAVLINK_COMM_3
|
||||||
MAVLINK_COMM_NB,
|
} mavlink_channel_t;
|
||||||
MAVLINK_COMM_NB_HIGH = 16
|
|
||||||
} 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
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
MAVLINK_PARSE_STATE_UNINIT=0,
|
MAVLINK_PARSE_STATE_UNINIT=0,
|
||||||
|
|
|
@ -34,12 +34,7 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus)
|
||||||
|
|
||||||
static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
|
static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
|
||||||
{
|
{
|
||||||
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
|
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
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];
|
return &m_mavlink_status[chan];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -73,7 +68,7 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t
|
||||||
msg->compid = component_id;
|
msg->compid = component_id;
|
||||||
// One sequence number per component
|
// One sequence number per component
|
||||||
msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq;
|
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);
|
checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN);
|
||||||
msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
|
msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte
|
||||||
msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
|
msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte
|
||||||
|
@ -202,11 +197,8 @@ 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 inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
||||||
{
|
{
|
||||||
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
|
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
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
|
// Initializes only once, values keep unchanged after first initialization
|
||||||
mavlink_parse_state_initialize(mavlink_get_channel_status(chan));
|
mavlink_parse_state_initialize(mavlink_get_channel_status(chan));
|
||||||
|
|
||||||
|
@ -391,23 +383,13 @@ 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 inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
||||||
{
|
{
|
||||||
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
|
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
|
static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2];
|
||||||
static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2];
|
static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH];
|
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH];
|
static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES];
|
||||||
static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES];
|
static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH];
|
static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
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
|
// Set a packet start candidate index if sign is start sign
|
||||||
if (c == MAVLINK_STX)
|
if (c == MAVLINK_STX)
|
||||||
|
@ -417,13 +399,8 @@ 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
|
// Parse normally, if a CRC mismatch occurs retry with the next packet index
|
||||||
}
|
}
|
||||||
//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
|
// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH];
|
// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
// 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
|
//// Initializes only once, values keep unchanged after first initialization
|
||||||
// mavlink_parse_state_initialize(&m_mavlink_status[chan]);
|
// mavlink_parse_state_initialize(&m_mavlink_status[chan]);
|
||||||
//
|
//
|
||||||
|
@ -575,6 +552,7 @@ typedef union __generic_64bit
|
||||||
{
|
{
|
||||||
uint8_t b[8];
|
uint8_t b[8];
|
||||||
int64_t ll; ///< Long long (64 bit)
|
int64_t ll; ///< Long long (64 bit)
|
||||||
|
double d; ///< IEEE-754 double precision floating point
|
||||||
} generic_64bit;
|
} generic_64bit;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -883,6 +861,103 @@ 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)
|
static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg)
|
||||||
{
|
{
|
||||||
|
|
Loading…
Reference in New Issue