fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 11:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta' into acro2

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-04 17:50:51 +01:00
parent 21300874ac 97c5daf924
commit b54b0efc29
25 changed files with 1416 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavlink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
+4 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_ahrs2.h
+287
View File
@@ -0,0 +1,287 @@
// MESSAGE AHRS2 PACKING
#define MAVLINK_MSG_ID_AHRS2 178
typedef struct __mavlink_ahrs2_t
{
float roll; ///< Roll angle (rad)
float pitch; ///< Pitch angle (rad)
float yaw; ///< Yaw angle (rad)
float altitude; ///< Altitude (MSL)
int32_t lat; ///< Latitude in degrees * 1E7
int32_t lng; ///< Longitude in degrees * 1E7
} mavlink_ahrs2_t;
#define MAVLINK_MSG_ID_AHRS2_LEN 24
#define MAVLINK_MSG_ID_178_LEN 24
#define MAVLINK_MSG_ID_AHRS2_CRC 47
#define MAVLINK_MSG_ID_178_CRC 47
#define MAVLINK_MESSAGE_INFO_AHRS2 { \
"AHRS2", \
6, \
{ { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs2_t, roll) }, \
{ "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs2_t, pitch) }, \
{ "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs2_t, yaw) }, \
{ "altitude", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs2_t, altitude) }, \
{ "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_ahrs2_t, lat) }, \
{ "lng", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_ahrs2_t, lng) }, \
} \
}
/**
* @brief Pack a ahrs2 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 roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param altitude Altitude (MSL)
* @param lat Latitude in degrees * 1E7
* @param lng Longitude in degrees * 1E7
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ahrs2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_AHRS2_LEN];
_mav_put_float(buf, 0, roll);
_mav_put_float(buf, 4, pitch);
_mav_put_float(buf, 8, yaw);
_mav_put_float(buf, 12, altitude);
_mav_put_int32_t(buf, 16, lat);
_mav_put_int32_t(buf, 20, lng);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN);
#else
mavlink_ahrs2_t packet;
packet.roll = roll;
packet.pitch = pitch;
packet.yaw = yaw;
packet.altitude = altitude;
packet.lat = lat;
packet.lng = lng;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_AHRS2;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS2_LEN);
#endif
}
/**
* @brief Pack a ahrs2 message on a channel
* @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 will be sent over
* @param msg The MAVLink message to compress the data into
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param altitude Altitude (MSL)
* @param lat Latitude in degrees * 1E7
* @param lng Longitude in degrees * 1E7
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ahrs2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
float roll,float pitch,float yaw,float altitude,int32_t lat,int32_t lng)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_AHRS2_LEN];
_mav_put_float(buf, 0, roll);
_mav_put_float(buf, 4, pitch);
_mav_put_float(buf, 8, yaw);
_mav_put_float(buf, 12, altitude);
_mav_put_int32_t(buf, 16, lat);
_mav_put_int32_t(buf, 20, lng);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS2_LEN);
#else
mavlink_ahrs2_t packet;
packet.roll = roll;
packet.pitch = pitch;
packet.yaw = yaw;
packet.altitude = altitude;
packet.lat = lat;
packet.lng = lng;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS2_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_AHRS2;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS2_LEN);
#endif
}
/**
* @brief Encode a ahrs2 struct
*
* @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 ahrs2 C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_ahrs2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2)
{
return mavlink_msg_ahrs2_pack(system_id, component_id, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng);
}
/**
* @brief Encode a ahrs2 struct on a channel
*
* @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 will be sent over
* @param msg The MAVLink message to compress the data into
* @param ahrs2 C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_ahrs2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs2_t* ahrs2)
{
return mavlink_msg_ahrs2_pack_chan(system_id, component_id, chan, msg, ahrs2->roll, ahrs2->pitch, ahrs2->yaw, ahrs2->altitude, ahrs2->lat, ahrs2->lng);
}
/**
* @brief Send a ahrs2 message
* @param chan MAVLink channel to send the message
*
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param altitude Altitude (MSL)
* @param lat Latitude in degrees * 1E7
* @param lng Longitude in degrees * 1E7
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_ahrs2_send(mavlink_channel_t chan, float roll, float pitch, float yaw, float altitude, int32_t lat, int32_t lng)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_AHRS2_LEN];
_mav_put_float(buf, 0, roll);
_mav_put_float(buf, 4, pitch);
_mav_put_float(buf, 8, yaw);
_mav_put_float(buf, 12, altitude);
_mav_put_int32_t(buf, 16, lat);
_mav_put_int32_t(buf, 20, lng);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, buf, MAVLINK_MSG_ID_AHRS2_LEN);
#endif
#else
mavlink_ahrs2_t packet;
packet.roll = roll;
packet.pitch = pitch;
packet.yaw = yaw;
packet.altitude = altitude;
packet.lat = lat;
packet.lng = lng;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN, MAVLINK_MSG_ID_AHRS2_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS2, (const char *)&packet, MAVLINK_MSG_ID_AHRS2_LEN);
#endif
#endif
}
#endif
// MESSAGE AHRS2 UNPACKING
/**
* @brief Get field roll from ahrs2 message
*
* @return Roll angle (rad)
*/
static inline float mavlink_msg_ahrs2_get_roll(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 0);
}
/**
* @brief Get field pitch from ahrs2 message
*
* @return Pitch angle (rad)
*/
static inline float mavlink_msg_ahrs2_get_pitch(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 4);
}
/**
* @brief Get field yaw from ahrs2 message
*
* @return Yaw angle (rad)
*/
static inline float mavlink_msg_ahrs2_get_yaw(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 8);
}
/**
* @brief Get field altitude from ahrs2 message
*
* @return Altitude (MSL)
*/
static inline float mavlink_msg_ahrs2_get_altitude(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 12);
}
/**
* @brief Get field lat from ahrs2 message
*
* @return Latitude in degrees * 1E7
*/
static inline int32_t mavlink_msg_ahrs2_get_lat(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 16);
}
/**
* @brief Get field lng from ahrs2 message
*
* @return Longitude in degrees * 1E7
*/
static inline int32_t mavlink_msg_ahrs2_get_lng(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 20);
}
/**
* @brief Decode a ahrs2 message into a struct
*
* @param msg The message to decode
* @param ahrs2 C-struct to decode the message contents into
*/
static inline void mavlink_msg_ahrs2_decode(const mavlink_message_t* msg, mavlink_ahrs2_t* ahrs2)
{
#if MAVLINK_NEED_BYTE_SWAP
ahrs2->roll = mavlink_msg_ahrs2_get_roll(msg);
ahrs2->pitch = mavlink_msg_ahrs2_get_pitch(msg);
ahrs2->yaw = mavlink_msg_ahrs2_get_yaw(msg);
ahrs2->altitude = mavlink_msg_ahrs2_get_altitude(msg);
ahrs2->lat = mavlink_msg_ahrs2_get_lat(msg);
ahrs2->lng = mavlink_msg_ahrs2_get_lng(msg);
#else
memcpy(ahrs2, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS2_LEN);
#endif
}
mavlink/include/mavlink/v1.0/ardupilotmega/testsuite.h
+54
View File
@@ -1398,6 +1398,59 @@ static void mavlink_test_rally_fetch_point(uint8_t system_id, uint8_t component_
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_ahrs2(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_ahrs2_t packet_in = {
17.0,
}45.0,
}73.0,
}101.0,
}963498296,
}963498504,
};
mavlink_ahrs2_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.roll = packet_in.roll;
packet1.pitch = packet_in.pitch;
packet1.yaw = packet_in.yaw;
packet1.altitude = packet_in.altitude;
packet1.lat = packet_in.lat;
packet1.lng = packet_in.lng;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_ahrs2_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_ahrs2_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_ahrs2_pack(system_id, component_id, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
mavlink_msg_ahrs2_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_ahrs2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
mavlink_msg_ahrs2_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_ahrs2_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_ahrs2_send(MAVLINK_COMM_1 , packet1.roll , packet1.pitch , packet1.yaw , packet1.altitude , packet1.lat , packet1.lng );
mavlink_msg_ahrs2_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_test_sensor_offsets(system_id, component_id, last_msg);
@@ -1426,6 +1479,7 @@ static void mavlink_test_ardupilotmega(uint8_t system_id, uint8_t component_id,
mavlink_test_airspeed_autocal(system_id, component_id, last_msg);
mavlink_test_rally_point(system_id, component_id, last_msg);
mavlink_test_rally_fetch_point(system_id, component_id, last_msg);
mavlink_test_ahrs2(system_id, component_id, last_msg);
}
#ifdef __cplusplus
mavlink/include/mavlink/v1.0/ardupilotmega/version.h
+2 -2
View File
@@ -5,8 +5,8 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:56:32 2013"
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:33 2014"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
#endif // MAVLINK_VERSION_H
mavlink/include/mavlink/v1.0/autoquad/autoquad.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/autoquad/version.h
+2 -2
View File
@@ -5,8 +5,8 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Dec 16 09:03:20 2013"
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:27:43 2014"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
#endif // MAVLINK_VERSION_H
mavlink/include/mavlink/v1.0/common/common.h
+7 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_data_transmission_handshake.h → mavlink/include/mavlink/v1.0/common/mavlink_msg_data_transmission_handshake.h
+34 -34
View File
@@ -1,23 +1,23 @@
// MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 193
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 130
typedef struct __mavlink_data_transmission_handshake_t
{
uint32_t size; ///< total data size in bytes (set on ACK only)
uint16_t width; ///< Width of a matrix or image
uint16_t height; ///< Height of a matrix or image
uint16_t packets; ///< number of packets beeing sent (set on ACK only)
uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)
uint8_t packets; ///< number of packets beeing sent (set on ACK only)
uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)
uint8_t jpg_quality; ///< JPEG quality out of [1,100]
} mavlink_data_transmission_handshake_t;
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 12
#define MAVLINK_MSG_ID_193_LEN 12
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 13
#define MAVLINK_MSG_ID_130_LEN 13
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 23
#define MAVLINK_MSG_ID_193_CRC 23
#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC 29
#define MAVLINK_MSG_ID_130_CRC 29
@@ -27,10 +27,10 @@ typedef struct __mavlink_data_transmission_handshake_t
{ { "size", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_data_transmission_handshake_t, size) }, \
{ "width", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_data_transmission_handshake_t, width) }, \
{ "height", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_data_transmission_handshake_t, height) }, \
{ "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, type) }, \
{ "packets", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_data_transmission_handshake_t, packets) }, \
{ "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, payload) }, \
{ "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \
{ "packets", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_data_transmission_handshake_t, packets) }, \
{ "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_data_transmission_handshake_t, type) }, \
{ "payload", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_data_transmission_handshake_t, payload) }, \
{ "jpg_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_data_transmission_handshake_t, jpg_quality) }, \
} \
}
@@ -51,17 +51,17 @@ typedef struct __mavlink_data_transmission_handshake_t
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint8_t packets, uint8_t payload, uint8_t jpg_quality)
uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
_mav_put_uint32_t(buf, 0, size);
_mav_put_uint16_t(buf, 4, width);
_mav_put_uint16_t(buf, 6, height);
_mav_put_uint8_t(buf, 8, type);
_mav_put_uint8_t(buf, 9, packets);
_mav_put_uint8_t(buf, 10, payload);
_mav_put_uint8_t(buf, 11, jpg_quality);
_mav_put_uint16_t(buf, 8, packets);
_mav_put_uint8_t(buf, 10, type);
_mav_put_uint8_t(buf, 11, payload);
_mav_put_uint8_t(buf, 12, jpg_quality);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
#else
@@ -69,8 +69,8 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst
packet.size = size;
packet.width = width;
packet.height = height;
packet.type = type;
packet.packets = packets;
packet.type = type;
packet.payload = payload;
packet.jpg_quality = jpg_quality;
@@ -102,17 +102,17 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst
*/
static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t type,uint32_t size,uint16_t width,uint16_t height,uint8_t packets,uint8_t payload,uint8_t jpg_quality)
uint8_t type,uint32_t size,uint16_t width,uint16_t height,uint16_t packets,uint8_t payload,uint8_t jpg_quality)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
_mav_put_uint32_t(buf, 0, size);
_mav_put_uint16_t(buf, 4, width);
_mav_put_uint16_t(buf, 6, height);
_mav_put_uint8_t(buf, 8, type);
_mav_put_uint8_t(buf, 9, packets);
_mav_put_uint8_t(buf, 10, payload);
_mav_put_uint8_t(buf, 11, jpg_quality);
_mav_put_uint16_t(buf, 8, packets);
_mav_put_uint8_t(buf, 10, type);
_mav_put_uint8_t(buf, 11, payload);
_mav_put_uint8_t(buf, 12, jpg_quality);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN);
#else
@@ -120,8 +120,8 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t
packet.size = size;
packet.width = width;
packet.height = height;
packet.type = type;
packet.packets = packets;
packet.type = type;
packet.payload = payload;
packet.jpg_quality = jpg_quality;
@@ -177,17 +177,17 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode_chan(uint8
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint8_t packets, uint8_t payload, uint8_t jpg_quality)
static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint16_t width, uint16_t height, uint16_t packets, uint8_t payload, uint8_t jpg_quality)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN];
_mav_put_uint32_t(buf, 0, size);
_mav_put_uint16_t(buf, 4, width);
_mav_put_uint16_t(buf, 6, height);
_mav_put_uint8_t(buf, 8, type);
_mav_put_uint8_t(buf, 9, packets);
_mav_put_uint8_t(buf, 10, payload);
_mav_put_uint8_t(buf, 11, jpg_quality);
_mav_put_uint16_t(buf, 8, packets);
_mav_put_uint8_t(buf, 10, type);
_mav_put_uint8_t(buf, 11, payload);
_mav_put_uint8_t(buf, 12, jpg_quality);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_CRC);
@@ -199,8 +199,8 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_
packet.size = size;
packet.width = width;
packet.height = height;
packet.type = type;
packet.packets = packets;
packet.type = type;
packet.payload = payload;
packet.jpg_quality = jpg_quality;
@@ -224,7 +224,7 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_
*/
static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 8);
return _MAV_RETURN_uint8_t(msg, 10);
}
/**
@@ -262,9 +262,9 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_get_height(const
*
* @return number of packets beeing sent (set on ACK only)
*/
static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
static inline uint16_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 9);
return _MAV_RETURN_uint16_t(msg, 8);
}
/**
@@ -274,7 +274,7 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const
*/
static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 10);
return _MAV_RETURN_uint8_t(msg, 11);
}
/**
@@ -284,7 +284,7 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const
*/
static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 11);
return _MAV_RETURN_uint8_t(msg, 12);
}
/**
@@ -299,8 +299,8 @@ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_
data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg);
data_transmission_handshake->width = mavlink_msg_data_transmission_handshake_get_width(msg);
data_transmission_handshake->height = mavlink_msg_data_transmission_handshake_get_height(msg);
data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg);
data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg);
data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg);
data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg);
data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg);
#else
mavlink/include/mavlink/v1.0/pixhawk/mavlink_msg_encapsulated_data.h → mavlink/include/mavlink/v1.0/common/mavlink_msg_encapsulated_data.h
+3 -3
View File
@@ -1,6 +1,6 @@
// MESSAGE ENCAPSULATED_DATA PACKING
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 194
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA 131
typedef struct __mavlink_encapsulated_data_t
{
@@ -9,10 +9,10 @@ typedef struct __mavlink_encapsulated_data_t
} mavlink_encapsulated_data_t;
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255
#define MAVLINK_MSG_ID_194_LEN 255
#define MAVLINK_MSG_ID_131_LEN 255
#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_CRC 223
#define MAVLINK_MSG_ID_194_CRC 223
#define MAVLINK_MSG_ID_131_CRC 223
#define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253
mavlink/include/mavlink/v1.0/common/mavlink_msg_gps2_raw.h
+419
View File
@@ -0,0 +1,419 @@
// MESSAGE GPS2_RAW PACKING
#define MAVLINK_MSG_ID_GPS2_RAW 124
typedef struct __mavlink_gps2_raw_t
{
uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
int32_t lat; ///< Latitude (WGS84), in degrees * 1E7
int32_t lon; ///< Longitude (WGS84), in degrees * 1E7
int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up)
uint32_t dgps_age; ///< Age of DGPS info
uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
uint16_t epv; ///< GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255
uint8_t dgps_numch; ///< Number of DGPS satellites
} mavlink_gps2_raw_t;
#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35
#define MAVLINK_MSG_ID_124_LEN 35
#define MAVLINK_MSG_ID_GPS2_RAW_CRC 87
#define MAVLINK_MSG_ID_124_CRC 87
#define MAVLINK_MESSAGE_INFO_GPS2_RAW { \
"GPS2_RAW", \
12, \
{ { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \
{ "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \
{ "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_gps2_raw_t, lon) }, \
{ "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_gps2_raw_t, alt) }, \
{ "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \
{ "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps2_raw_t, eph) }, \
{ "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps2_raw_t, epv) }, \
{ "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gps2_raw_t, vel) }, \
{ "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_gps2_raw_t, cog) }, \
{ "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \
{ "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \
{ "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \
} \
}
/**
* @brief Pack a gps2_raw 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 time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
* @param lat Latitude (WGS84), in degrees * 1E7
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
* @param satellites_visible Number of satellites visible. If unknown, set to 255
* @param dgps_numch Number of DGPS satellites
* @param dgps_age Age of DGPS info
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN];
_mav_put_uint64_t(buf, 0, time_usec);
_mav_put_int32_t(buf, 8, lat);
_mav_put_int32_t(buf, 12, lon);
_mav_put_int32_t(buf, 16, alt);
_mav_put_uint32_t(buf, 20, dgps_age);
_mav_put_uint16_t(buf, 24, eph);
_mav_put_uint16_t(buf, 26, epv);
_mav_put_uint16_t(buf, 28, vel);
_mav_put_uint16_t(buf, 30, cog);
_mav_put_uint8_t(buf, 32, fix_type);
_mav_put_uint8_t(buf, 33, satellites_visible);
_mav_put_uint8_t(buf, 34, dgps_numch);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#else
mavlink_gps2_raw_t packet;
packet.time_usec = time_usec;
packet.lat = lat;
packet.lon = lon;
packet.alt = alt;
packet.dgps_age = dgps_age;
packet.eph = eph;
packet.epv = epv;
packet.vel = vel;
packet.cog = cog;
packet.fix_type = fix_type;
packet.satellites_visible = satellites_visible;
packet.dgps_numch = dgps_numch;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_GPS2_RAW;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
}
/**
* @brief Pack a gps2_raw message on a channel
* @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 will be sent over
* @param msg The MAVLink message to compress the data into
* @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
* @param lat Latitude (WGS84), in degrees * 1E7
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
* @param satellites_visible Number of satellites visible. If unknown, set to 255
* @param dgps_numch Number of DGPS satellites
* @param dgps_age Age of DGPS info
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN];
_mav_put_uint64_t(buf, 0, time_usec);
_mav_put_int32_t(buf, 8, lat);
_mav_put_int32_t(buf, 12, lon);
_mav_put_int32_t(buf, 16, alt);
_mav_put_uint32_t(buf, 20, dgps_age);
_mav_put_uint16_t(buf, 24, eph);
_mav_put_uint16_t(buf, 26, epv);
_mav_put_uint16_t(buf, 28, vel);
_mav_put_uint16_t(buf, 30, cog);
_mav_put_uint8_t(buf, 32, fix_type);
_mav_put_uint8_t(buf, 33, satellites_visible);
_mav_put_uint8_t(buf, 34, dgps_numch);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#else
mavlink_gps2_raw_t packet;
packet.time_usec = time_usec;
packet.lat = lat;
packet.lon = lon;
packet.alt = alt;
packet.dgps_age = dgps_age;
packet.eph = eph;
packet.epv = epv;
packet.vel = vel;
packet.cog = cog;
packet.fix_type = fix_type;
packet.satellites_visible = satellites_visible;
packet.dgps_numch = dgps_numch;
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_GPS2_RAW;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
}
/**
* @brief Encode a gps2_raw struct
*
* @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 gps2_raw C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw)
{
return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age);
}
/**
* @brief Encode a gps2_raw struct on a channel
*
* @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 will be sent over
* @param msg The MAVLink message to compress the data into
* @param gps2_raw C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw)
{
return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age);
}
/**
* @brief Send a gps2_raw message
* @param chan MAVLink channel to send the message
*
* @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
* @param lat Latitude (WGS84), in degrees * 1E7
* @param lon Longitude (WGS84), in degrees * 1E7
* @param alt Altitude (WGS84), in meters * 1000 (positive for up)
* @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param epv GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
* @param vel GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
* @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
* @param satellites_visible Number of satellites visible. If unknown, set to 255
* @param dgps_numch Number of DGPS satellites
* @param dgps_age Age of DGPS info
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN];
_mav_put_uint64_t(buf, 0, time_usec);
_mav_put_int32_t(buf, 8, lat);
_mav_put_int32_t(buf, 12, lon);
_mav_put_int32_t(buf, 16, alt);
_mav_put_uint32_t(buf, 20, dgps_age);
_mav_put_uint16_t(buf, 24, eph);
_mav_put_uint16_t(buf, 26, epv);
_mav_put_uint16_t(buf, 28, vel);
_mav_put_uint16_t(buf, 30, cog);
_mav_put_uint8_t(buf, 32, fix_type);
_mav_put_uint8_t(buf, 33, satellites_visible);
_mav_put_uint8_t(buf, 34, dgps_numch);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
#else
mavlink_gps2_raw_t packet;
packet.time_usec = time_usec;
packet.lat = lat;
packet.lon = lon;
packet.alt = alt;
packet.dgps_age = dgps_age;
packet.eph = eph;
packet.epv = epv;
packet.vel = vel;
packet.cog = cog;
packet.fix_type = fix_type;
packet.satellites_visible = satellites_visible;
packet.dgps_numch = dgps_numch;
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
#endif
}
#endif
// MESSAGE GPS2_RAW UNPACKING
/**
* @brief Get field time_usec from gps2_raw message
*
* @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
*/
static inline uint64_t mavlink_msg_gps2_raw_get_time_usec(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint64_t(msg, 0);
}
/**
* @brief Get field fix_type from gps2_raw message
*
* @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
*/
static inline uint8_t mavlink_msg_gps2_raw_get_fix_type(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 32);
}
/**
* @brief Get field lat from gps2_raw message
*
* @return Latitude (WGS84), in degrees * 1E7
*/
static inline int32_t mavlink_msg_gps2_raw_get_lat(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 8);
}
/**
* @brief Get field lon from gps2_raw message
*
* @return Longitude (WGS84), in degrees * 1E7
*/
static inline int32_t mavlink_msg_gps2_raw_get_lon(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 12);
}
/**
* @brief Get field alt from gps2_raw message
*
* @return Altitude (WGS84), in meters * 1000 (positive for up)
*/
static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 16);
}
/**
* @brief Get field eph from gps2_raw message
*
* @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
*/
static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 24);
}
/**
* @brief Get field epv from gps2_raw message
*
* @return GPS VDOP vertical dilution of position in cm (m*100). If unknown, set to: UINT16_MAX
*/
static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 26);
}
/**
* @brief Get field vel from gps2_raw message
*
* @return GPS ground speed (m/s * 100). If unknown, set to: UINT16_MAX
*/
static inline uint16_t mavlink_msg_gps2_raw_get_vel(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 28);
}
/**
* @brief Get field cog from gps2_raw message
*
* @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX
*/
static inline uint16_t mavlink_msg_gps2_raw_get_cog(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 30);
}
/**
* @brief Get field satellites_visible from gps2_raw message
*
* @return Number of satellites visible. If unknown, set to 255
*/
static inline uint8_t mavlink_msg_gps2_raw_get_satellites_visible(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 33);
}
/**
* @brief Get field dgps_numch from gps2_raw message
*
* @return Number of DGPS satellites
*/
static inline uint8_t mavlink_msg_gps2_raw_get_dgps_numch(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 34);
}
/**
* @brief Get field dgps_age from gps2_raw message
*
* @return Age of DGPS info
*/
static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 20);
}
/**
* @brief Decode a gps2_raw message into a struct
*
* @param msg The message to decode
* @param gps2_raw C-struct to decode the message contents into
*/
static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mavlink_gps2_raw_t* gps2_raw)
{
#if MAVLINK_NEED_BYTE_SWAP
gps2_raw->time_usec = mavlink_msg_gps2_raw_get_time_usec(msg);
gps2_raw->lat = mavlink_msg_gps2_raw_get_lat(msg);
gps2_raw->lon = mavlink_msg_gps2_raw_get_lon(msg);
gps2_raw->alt = mavlink_msg_gps2_raw_get_alt(msg);
gps2_raw->dgps_age = mavlink_msg_gps2_raw_get_dgps_age(msg);
gps2_raw->eph = mavlink_msg_gps2_raw_get_eph(msg);
gps2_raw->epv = mavlink_msg_gps2_raw_get_epv(msg);
gps2_raw->vel = mavlink_msg_gps2_raw_get_vel(msg);
gps2_raw->cog = mavlink_msg_gps2_raw_get_cog(msg);
gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg);
gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg);
gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg);
#else
memcpy(gps2_raw, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS2_RAW_LEN);
#endif
}
mavlink/include/mavlink/v1.0/common/mavlink_msg_gps_inject_data.h
+237
View File
@@ -0,0 +1,237 @@
// MESSAGE GPS_INJECT_DATA PACKING
#define MAVLINK_MSG_ID_GPS_INJECT_DATA 123
typedef struct __mavlink_gps_inject_data_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t len; ///< data length
uint8_t data[110]; ///< raw data (110 is enough for 12 satellites of RTCMv2)
} mavlink_gps_inject_data_t;
#define MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN 113
#define MAVLINK_MSG_ID_123_LEN 113
#define MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC 250
#define MAVLINK_MSG_ID_123_CRC 250
#define MAVLINK_MSG_GPS_INJECT_DATA_FIELD_DATA_LEN 110
#define MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA { \
"GPS_INJECT_DATA", \
4, \
{ { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_inject_data_t, target_system) }, \
{ "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_inject_data_t, target_component) }, \
{ "len", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_gps_inject_data_t, len) }, \
{ "data", NULL, MAVLINK_TYPE_UINT8_T, 110, 3, offsetof(mavlink_gps_inject_data_t, data) }, \
} \
}
/**
* @brief Pack a gps_inject_data 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 target_system System ID
* @param target_component Component ID
* @param len data length
* @param data raw data (110 is enough for 12 satellites of RTCMv2)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_gps_inject_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
_mav_put_uint8_t(buf, 2, len);
_mav_put_uint8_t_array(buf, 3, data, 110);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#else
mavlink_gps_inject_data_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
packet.len = len;
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
#else
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
}
/**
* @brief Pack a gps_inject_data message on a channel
* @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 will be sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param len data length
* @param data raw data (110 is enough for 12 satellites of RTCMv2)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_gps_inject_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
uint8_t target_system,uint8_t target_component,uint8_t len,const uint8_t *data)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
_mav_put_uint8_t(buf, 2, len);
_mav_put_uint8_t_array(buf, 3, data, 110);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#else
mavlink_gps_inject_data_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
packet.len = len;
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
msg->msgid = MAVLINK_MSG_ID_GPS_INJECT_DATA;
#if MAVLINK_CRC_EXTRA
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
#else
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
}
/**
* @brief Encode a gps_inject_data struct
*
* @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 gps_inject_data C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_gps_inject_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data)
{
return mavlink_msg_gps_inject_data_pack(system_id, component_id, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data);
}
/**
* @brief Encode a gps_inject_data struct on a channel
*
* @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 will be sent over
* @param msg The MAVLink message to compress the data into
* @param gps_inject_data C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_gps_inject_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_inject_data_t* gps_inject_data)
{
return mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, chan, msg, gps_inject_data->target_system, gps_inject_data->target_component, gps_inject_data->len, gps_inject_data->data);
}
/**
* @brief Send a gps_inject_data message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param len data length
* @param data raw data (110 is enough for 12 satellites of RTCMv2)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_inject_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t len, const uint8_t *data)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN];
_mav_put_uint8_t(buf, 0, target_system);
_mav_put_uint8_t(buf, 1, target_component);
_mav_put_uint8_t(buf, 2, len);
_mav_put_uint8_t_array(buf, 3, data, 110);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, buf, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
#else
mavlink_gps_inject_data_t packet;
packet.target_system = target_system;
packet.target_component = target_component;
packet.len = len;
mav_array_memcpy(packet.data, data, sizeof(uint8_t)*110);
#if MAVLINK_CRC_EXTRA
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN, MAVLINK_MSG_ID_GPS_INJECT_DATA_CRC);
#else
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INJECT_DATA, (const char *)&packet, MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
#endif
}
#endif
// MESSAGE GPS_INJECT_DATA UNPACKING
/**
* @brief Get field target_system from gps_inject_data message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_gps_inject_data_get_target_system(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 0);
}
/**
* @brief Get field target_component from gps_inject_data message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_gps_inject_data_get_target_component(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 1);
}
/**
* @brief Get field len from gps_inject_data message
*
* @return data length
*/
static inline uint8_t mavlink_msg_gps_inject_data_get_len(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 2);
}
/**
* @brief Get field data from gps_inject_data message
*
* @return raw data (110 is enough for 12 satellites of RTCMv2)
*/
static inline uint16_t mavlink_msg_gps_inject_data_get_data(const mavlink_message_t* msg, uint8_t *data)
{
return _MAV_RETURN_uint8_t_array(msg, data, 110, 3);
}
/**
* @brief Decode a gps_inject_data message into a struct
*
* @param msg The message to decode
* @param gps_inject_data C-struct to decode the message contents into
*/
static inline void mavlink_msg_gps_inject_data_decode(const mavlink_message_t* msg, mavlink_gps_inject_data_t* gps_inject_data)
{
#if MAVLINK_NEED_BYTE_SWAP
gps_inject_data->target_system = mavlink_msg_gps_inject_data_get_target_system(msg);
gps_inject_data->target_component = mavlink_msg_gps_inject_data_get_target_component(msg);
gps_inject_data->len = mavlink_msg_gps_inject_data_get_len(msg);
mavlink_msg_gps_inject_data_get_data(msg, gps_inject_data->data);
#else
memcpy(gps_inject_data, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_GPS_INJECT_DATA_LEN);
#endif
}
mavlink/include/mavlink/v1.0/common/testsuite.h
+218
View File
@@ -4839,6 +4839,220 @@ static void mavlink_test_log_request_end(uint8_t system_id, uint8_t component_id
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_gps_inject_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_gps_inject_data_t packet_in = {
5,
}72,
}139,
}{ 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59 },
};
mavlink_gps_inject_data_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.target_system = packet_in.target_system;
packet1.target_component = packet_in.target_component;
packet1.len = packet_in.len;
mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*110);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps_inject_data_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_gps_inject_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps_inject_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
mavlink_msg_gps_inject_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps_inject_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
mavlink_msg_gps_inject_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_gps_inject_data_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps_inject_data_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.len , packet1.data );
mavlink_msg_gps_inject_data_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_gps2_raw_t packet_in = {
93372036854775807ULL,
}963497880,
}963498088,
}963498296,
}963498504,
}18483,
}18587,
}18691,
}18795,
}101,
}168,
}235,
};
mavlink_gps2_raw_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.time_usec = packet_in.time_usec;
packet1.lat = packet_in.lat;
packet1.lon = packet_in.lon;
packet1.alt = packet_in.alt;
packet1.dgps_age = packet_in.dgps_age;
packet1.eph = packet_in.eph;
packet1.epv = packet_in.epv;
packet1.vel = packet_in.vel;
packet1.cog = packet_in.cog;
packet1.fix_type = packet_in.fix_type;
packet1.satellites_visible = packet_in.satellites_visible;
packet1.dgps_numch = packet_in.dgps_numch;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps2_raw_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_gps2_raw_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
mavlink_msg_gps2_raw_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
mavlink_msg_gps2_raw_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_gps2_raw_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age );
mavlink_msg_gps2_raw_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_data_transmission_handshake_t packet_in = {
963497464,
}17443,
}17547,
}17651,
}163,
}230,
}41,
};
mavlink_data_transmission_handshake_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.size = packet_in.size;
packet1.width = packet_in.width;
packet1.height = packet_in.height;
packet1.packets = packet_in.packets;
packet1.type = packet_in.type;
packet1.payload = packet_in.payload;
packet1.jpg_quality = packet_in.jpg_quality;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_encapsulated_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_encapsulated_data_t packet_in = {
17235,
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135 },
};
mavlink_encapsulated_data_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.seqnr = packet_in.seqnr;
mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*253);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_pack(system_id, component_id, &msg , packet1.seqnr , packet1.data );
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seqnr , packet1.data );
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_encapsulated_data_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_1 , packet1.seqnr , packet1.data );
mavlink_msg_encapsulated_data_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
@@ -5393,6 +5607,10 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
mavlink_test_log_data(system_id, component_id, last_msg);
mavlink_test_log_erase(system_id, component_id, last_msg);
mavlink_test_log_request_end(system_id, component_id, last_msg);
mavlink_test_gps_inject_data(system_id, component_id, last_msg);
mavlink_test_gps2_raw(system_id, component_id, last_msg);
mavlink_test_data_transmission_handshake(system_id, component_id, last_msg);
mavlink_test_encapsulated_data(system_id, component_id, last_msg);
mavlink_test_battery_status(system_id, component_id, last_msg);
mavlink_test_setpoint_8dof(system_id, component_id, last_msg);
mavlink_test_setpoint_6dof(system_id, component_id, last_msg);
mavlink/include/mavlink/v1.0/common/version.h
+2 -2
View File
@@ -5,8 +5,8 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:59:18 2013"
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:29 2014"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
#endif // MAVLINK_VERSION_H
mavlink/include/mavlink/v1.0/matrixpilot/matrixpilot.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/matrixpilot/version.h
+2 -2
View File
@@ -5,8 +5,8 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:57:49 2013"
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:04 2014"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
#endif // MAVLINK_VERSION_H
mavlink/include/mavlink/v1.0/mavlink_types.h
+1 -1
View File
@@ -48,7 +48,7 @@ typedef struct __mavlink_system {
uint8_t type; ///< Unused, can be used by user to store the system's type
uint8_t state; ///< Unused, can be used by user to store the system's state
uint8_t mode; ///< Unused, can be used by user to store the system's mode
uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
uint32_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
} mavlink_system_t;
typedef struct __mavlink_message {
mavlink/include/mavlink/v1.0/pixhawk/pixhawk.h
+3 -5
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/pixhawk/testsuite.h
-102
View File
@@ -853,106 +853,6 @@ static void mavlink_test_point_of_interest_connection(uint8_t system_id, uint8_t
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_data_transmission_handshake(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_data_transmission_handshake_t packet_in = {
963497464,
}17443,
}17547,
}29,
}96,
}163,
}230,
};
mavlink_data_transmission_handshake_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.size = packet_in.size;
packet1.width = packet_in.width;
packet1.height = packet_in.height;
packet1.type = packet_in.type;
packet1.packets = packet_in.packets;
packet1.payload = packet_in.payload;
packet1.jpg_quality = packet_in.jpg_quality;
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_pack(system_id, component_id, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
mavlink_msg_data_transmission_handshake_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_data_transmission_handshake_send(MAVLINK_COMM_1 , packet1.type , packet1.size , packet1.width , packet1.height , packet1.packets , packet1.payload , packet1.jpg_quality );
mavlink_msg_data_transmission_handshake_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_encapsulated_data(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_encapsulated_data_t packet_in = {
17235,
}{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135 },
};
mavlink_encapsulated_data_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.seqnr = packet_in.seqnr;
mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*253);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_pack(system_id, component_id, &msg , packet1.seqnr , packet1.data );
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seqnr , packet1.data );
mavlink_msg_encapsulated_data_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_encapsulated_data_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_encapsulated_data_send(MAVLINK_COMM_1 , packet1.seqnr , packet1.data );
mavlink_msg_encapsulated_data_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_brief_feature(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
@@ -1086,8 +986,6 @@ static void mavlink_test_pixhawk(uint8_t system_id, uint8_t component_id, mavlin
mavlink_test_pattern_detected(system_id, component_id, last_msg);
mavlink_test_point_of_interest(system_id, component_id, last_msg);
mavlink_test_point_of_interest_connection(system_id, component_id, last_msg);
mavlink_test_data_transmission_handshake(system_id, component_id, last_msg);
mavlink_test_encapsulated_data(system_id, component_id, last_msg);
mavlink_test_brief_feature(system_id, component_id, last_msg);
mavlink_test_attitude_control(system_id, component_id, last_msg);
}
mavlink/include/mavlink/v1.0/pixhawk/version.h
+1 -1
View File
@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Dec 16 08:58:02 2013"
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:16 2014"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
mavlink/include/mavlink/v1.0/sensesoar/sensesoar.h
+3 -3
View File
File diff suppressed because one or more lines are too long
mavlink/include/mavlink/v1.0/sensesoar/testsuite.h
+13 -13
View File
@@ -31,8 +31,8 @@ static void mavlink_test_obs_position(uint8_t system_id, uint8_t component_id, m
uint16_t i;
mavlink_obs_position_t packet_in = {
963497464,
963497672,
963497880,
}963497672,
}963497880,
};
mavlink_obs_position_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -207,8 +207,8 @@ static void mavlink_test_obs_air_velocity(uint8_t system_id, uint8_t component_i
uint16_t i;
mavlink_obs_air_velocity_t packet_in = {
17.0,
45.0,
73.0,
}45.0,
}73.0,
};
mavlink_obs_air_velocity_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -254,7 +254,7 @@ static void mavlink_test_obs_bias(uint8_t system_id, uint8_t component_id, mavli
uint16_t i;
mavlink_obs_bias_t packet_in = {
{ 17.0, 18.0, 19.0 },
{ 101.0, 102.0, 103.0 },
}{ 101.0, 102.0, 103.0 },
};
mavlink_obs_bias_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -428,7 +428,7 @@ static void mavlink_test_llc_out(uint8_t system_id, uint8_t component_id, mavlin
uint16_t i;
mavlink_llc_out_t packet_in = {
{ 17235, 17236, 17237, 17238 },
{ 17651, 17652 },
}{ 17651, 17652 },
};
mavlink_llc_out_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -473,8 +473,8 @@ static void mavlink_test_pm_elec(uint8_t system_id, uint8_t component_id, mavlin
uint16_t i;
mavlink_pm_elec_t packet_in = {
17.0,
45.0,
{ 73.0, 74.0, 75.0 },
}45.0,
}{ 73.0, 74.0, 75.0 },
};
mavlink_pm_elec_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -520,9 +520,9 @@ static void mavlink_test_sys_stat(uint8_t system_id, uint8_t component_id, mavli
uint16_t i;
mavlink_sys_stat_t packet_in = {
5,
72,
139,
206,
}72,
}139,
}206,
};
mavlink_sys_stat_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -569,7 +569,7 @@ static void mavlink_test_cmd_airspeed_chng(uint8_t system_id, uint8_t component_
uint16_t i;
mavlink_cmd_airspeed_chng_t packet_in = {
17.0,
17,
}17,
};
mavlink_cmd_airspeed_chng_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
@@ -614,7 +614,7 @@ static void mavlink_test_cmd_airspeed_ack(uint8_t system_id, uint8_t component_i
uint16_t i;
mavlink_cmd_airspeed_ack_t packet_in = {
17.0,
17,
}17,
};
mavlink_cmd_airspeed_ack_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
mavlink/include/mavlink/v1.0/sensesoar/version.h
+2 -2
View File
@@ -5,8 +5,8 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Tue Sep 10 23:50:02 2013"
#define MAVLINK_BUILD_DATE "Tue Feb 4 15:28:27 2014"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
#endif // MAVLINK_VERSION_H
src/modules/commander/commander.cpp
+4
View File
@@ -1965,6 +1965,10 @@ void *commander_low_prio_loop(void *arg)
break;
}
case VEHICLE_CMD_START_RX_PAIR:
/* handled in the IO driver */
break;
default:
/* don't answer on unsupported commands, it will be done in main loop */
break;
src/modules/dataman/dataman.c
+108 -70
View File
@@ -126,45 +126,46 @@ static const char *k_data_manager_device_path = "/fs/microsd/dataman";
/* The data manager work queues */
typedef struct {
sq_queue_t q;
sem_t mutex; /* Mutual exclusion on work queue adds and deletes */
unsigned size;
unsigned max_size;
sq_queue_t q; /* Nuttx queue */
sem_t mutex; /* Mutual exclusion on work queue adds and deletes */
unsigned size; /* Current size of queue */
unsigned max_size; /* Maximum queue size reached */
} work_q_t;
static work_q_t g_free_q;
static work_q_t g_work_q;
static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/
static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */
sem_t g_work_queued_sema;
sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */
sem_t g_init_sema;
static bool g_task_should_exit; /**< if true, dataman task should exit */
#define DM_SECTOR_HDR_SIZE 4
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
static void init_q(work_q_t *q)
{
sq_init(&(q->q));
sem_init(&(q->mutex), 1, 1);
q->size = q->max_size = 0;
sq_init(&(q->q)); /* Initialize the NuttX queue structure */
sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */
q->size = q->max_size = 0; /* Queue is initially empty */
}
static void destroy_q(work_q_t *q)
static inline void
destroy_q(work_q_t *q)
{
sem_destroy(&(q->mutex));
sem_destroy(&(q->mutex)); /* Destroy the queue lock */
}
static inline void
lock_queue(work_q_t *q)
{
sem_wait(&(q->mutex));
sem_wait(&(q->mutex)); /* Acquire the queue lock */
}
static inline void
unlock_queue(work_q_t *q)
{
sem_post(&(q->mutex));
sem_post(&(q->mutex)); /* Release the queue lock */
}
static work_q_item_t *
@@ -172,54 +173,47 @@ create_work_item(void)
{
work_q_item_t *item;
/* Try to reuse item from free item queue */
lock_queue(&g_free_q);
if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
g_free_q.size--;
unlock_queue(&g_free_q);
/* If we there weren't any free items then obtain memory for a new one */
if (item == NULL)
item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
/* If we got one then lock the item*/
if (item)
sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
/* return the item pointer, or NULL if all failed */
return item;
}
/* Work queue management functions */
static void
enqueue_work_item(work_q_item_t *item)
{
/* put the work item on the work queue */
lock_queue(&g_work_q);
sq_addlast(&item->link, &(g_work_q.q));
if (++g_work_q.size > g_work_q.max_size)
g_work_q.max_size = g_work_q.size;
unlock_queue(&g_work_q);
/* tell the work thread that work is available */
sem_post(&g_work_queued_sema);
}
static void
static inline void
destroy_work_item(work_q_item_t *item)
{
sem_destroy(&item->wait_sem);
sem_destroy(&item->wait_sem); /* Destroy the item lock */
/* Return the item to the free item queue for later reuse */
lock_queue(&g_free_q);
sq_addfirst(&item->link, &(g_free_q.q));
/* Update the queue size and potentially the maximum queue size */
if (++g_free_q.size > g_free_q.max_size)
g_free_q.max_size = g_free_q.size;
unlock_queue(&g_free_q);
}
static work_q_item_t *
static inline work_q_item_t *
dequeue_work_item(void)
{
work_q_item_t *work;
/* retrieve the 1st item on the work queue */
lock_queue(&g_work_q);
if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
@@ -229,6 +223,32 @@ dequeue_work_item(void)
return work;
}
static int
enqueue_work_item_and_wait_for_result(work_q_item_t *item)
{
/* put the work item at the end of the work queue */
lock_queue(&g_work_q);
sq_addlast(&item->link, &(g_work_q.q));
/* Adjust the queue size and potentially the maximum queue size */
if (++g_work_q.size > g_work_q.max_size)
g_work_q.max_size = g_work_q.size;
unlock_queue(&g_work_q);
/* tell the work thread that work is available */
sem_post(&g_work_queued_sema);
/* wait for the result */
sem_wait(&item->wait_sem);
int result = item->result;
destroy_work_item(item);
return result;
}
/* Calculate the offset in file of specific item */
static int
calculate_offset(dm_item_t item, unsigned char index)
@@ -250,6 +270,8 @@ calculate_offset(dm_item_t item, unsigned char index)
*
* byte 0: Length of user data item
* byte 1: Persistence of this data item
* byte 2: Unused (for future use)
* byte 3: Unused (for future use)
* byte DM_SECTOR_HDR_SIZE... : data item value
*
* The total size must not exceed k_sector_size
@@ -266,6 +288,7 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
/* Get the offset for this item */
offset = calculate_offset(item, index);
/* If item type or index out of range, return error */
if (offset < 0)
return -1;
@@ -283,10 +306,12 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
len = -1;
/* Seek to the right spot in the data manager file and write the data item */
if (lseek(g_task_fd, offset, SEEK_SET) == offset)
if ((len = write(g_task_fd, buffer, count)) == count)
fsync(g_task_fd);
fsync(g_task_fd); /* Make sure data is written to physical media */
/* Make sure the write succeeded */
if (len != count)
return -1;
@@ -304,6 +329,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
/* Get the offset for this item */
offset = calculate_offset(item, index);
/* If item type or index out of range, return error */
if (offset < 0)
return -1;
@@ -316,14 +342,17 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
if (lseek(g_task_fd, offset, SEEK_SET) == offset)
len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
/* Check for length issues */
/* Check for read error */
if (len < 0)
return -1;
/* A zero length entry is a empty entry */
if (len == 0)
buffer[0] = 0;
/* See if we got data */
if (buffer[0] > 0) {
/* We got more than requested!!! */
if (buffer[0] > count)
return -1;
@@ -340,11 +369,14 @@ _clear(dm_item_t item)
{
int i, result = 0;
/* Get the offset of 1st item of this type */
int offset = calculate_offset(item, 0);
/* Check for item type out of range */
if (offset < 0)
return -1;
/* Clear all items of this type */
for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
char buf[1];
@@ -353,9 +385,11 @@ _clear(dm_item_t item)
break;
}
/* Avoid SD flash wear by only doing writes where necessary */
if (read(g_task_fd, buf, 1) < 1)
break;
/* If item has length greater than 0 it needs to be overwritten */
if (buf[0]) {
if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
result = -1;
@@ -373,6 +407,7 @@ _clear(dm_item_t item)
offset += k_sector_size;
}
/* Make sure data is actually written to physical media */
fsync(g_task_fd);
return result;
}
@@ -452,12 +487,13 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
{
work_q_item_t *work;
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit)
return -1;
/* Will return with queues locked */
/* get a work item and queue up a write request */
if ((work = create_work_item()) == NULL)
return -1; /* queues unlocked on failure */
return -1;
work->func = dm_write_func;
work->write_params.item = item;
@@ -465,12 +501,9 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
work->write_params.persistence = persistence;
work->write_params.buf = buf;
work->write_params.count = count;
enqueue_work_item(work);
sem_wait(&work->wait_sem);
ssize_t result = work->result;
destroy_work_item(work);
return result;
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}
/* Retrieve from the data manager file */
@@ -479,24 +512,22 @@ dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{
work_q_item_t *work;
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit)
return -1;
/* Will return with queues locked */
/* get a work item and queue up a read request */
if ((work = create_work_item()) == NULL)
return -1; /* queues unlocked on failure */
return -1;
work->func = dm_read_func;
work->read_params.item = item;
work->read_params.index = index;
work->read_params.buf = buf;
work->read_params.count = count;
enqueue_work_item(work);
sem_wait(&work->wait_sem);
ssize_t result = work->result;
destroy_work_item(work);
return result;
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}
__EXPORT int
@@ -504,21 +535,19 @@ dm_clear(dm_item_t item)
{
work_q_item_t *work;
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit)
return -1;
/* Will return with queues locked */
/* get a work item and queue up a clear request */
if ((work = create_work_item()) == NULL)
return -1; /* queues unlocked on failure */
return -1;
work->func = dm_clear_func;
work->clear_params.item = item;
enqueue_work_item(work);
sem_wait(&work->wait_sem);
int result = work->result;
destroy_work_item(work);
return result;
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
return enqueue_work_item_and_wait_for_result(work);
}
/* Tell the data manager about the type of the last reset */
@@ -527,21 +556,19 @@ dm_restart(dm_reset_reason reason)
{
work_q_item_t *work;
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit)
return -1;
/* Will return with queues locked */
/* get a work item and queue up a restart request */
if ((work = create_work_item()) == NULL)
return -1; /* queues unlocked on failure */
return -1;
work->func = dm_restart_func;
work->restart_params.reason = reason;
enqueue_work_item(work);
sem_wait(&work->wait_sem);
int result = work->result;
destroy_work_item(work);
return result;
/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
return enqueue_work_item_and_wait_for_result(work);
}
static int
@@ -570,24 +597,29 @@ task_main(int argc, char *argv[])
sem_init(&g_work_queued_sema, 1, 0);
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
if (g_task_fd < 0) {
warnx("Could not open data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema);
sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
warnx("Could not seek data manager file %s", k_data_manager_device_path);
sem_post(&g_init_sema);
sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
fsync(g_task_fd);
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
/* worker thread is shutting down but still processing requests */
g_fd = g_task_fd;
warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
/* Tell startup that the worker thread has completed its initialization */
sem_post(&g_init_sema);
/* Start the endless loop, waiting for then processing work requests */
@@ -595,7 +627,7 @@ task_main(int argc, char *argv[])
/* do we need to exit ??? */
if ((g_task_should_exit) && (g_fd >= 0)) {
/* Close the file handle to stop further queueing */
/* Close the file handle to stop further queuing */
g_fd = -1;
}
@@ -607,6 +639,7 @@ task_main(int argc, char *argv[])
/* Empty the work queue */
while ((work = dequeue_work_item())) {
/* handle each work item with the appropriate handler */
switch (work->func) {
case dm_write_func:
g_func_counts[dm_write_func]++;
@@ -647,7 +680,7 @@ task_main(int argc, char *argv[])
close(g_task_fd);
g_task_fd = -1;
/* Empty the work queue */
/* The work queue is now empty, empty the free queue */
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
break;
@@ -669,7 +702,7 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the task */
/* start the worker thread */
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
@@ -704,7 +737,7 @@ stop(void)
static void
usage(void)
{
errx(1, "usage: dataman {start|stop|status}");
errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
}
int
@@ -726,6 +759,7 @@ dataman_main(int argc, char *argv[])
exit(0);
}
/* Worker thread should be running for all other commands */
if (g_fd < 0)
errx(1, "not running");
@@ -733,6 +767,10 @@ dataman_main(int argc, char *argv[])
stop();
else if (!strcmp(argv[1], "status"))
status();
else if (!strcmp(argv[1], "poweronrestart"))
dm_restart(DM_INIT_REASON_POWER_ON);
else if (!strcmp(argv[1], "inflightrestart"))
dm_restart(DM_INIT_REASON_IN_FLIGHT);
else
usage();
src/systemcmds/tests/test_dataman.c
+4 -4
View File
@@ -89,7 +89,7 @@ task_main(int argc, char *argv[])
unsigned hash = i ^ my_id;
unsigned len = (hash & 63) + 2;
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
warnx("ret: %d", ret);
if (ret != len) {
warnx("%d write failed, index %d, length %d", my_id, hash, len);
@@ -103,7 +103,7 @@ task_main(int argc, char *argv[])
for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
unsigned hash = i ^ my_id;
unsigned len2, len = (hash & 63) + 2;
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD, hash, buffer, sizeof(buffer))) < 2) {
if ((len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer))) < 2) {
warnx("%d read failed length test, index %d", my_id, hash);
goto fail;
}
@@ -163,7 +163,7 @@ int test_dataman(int argc, char *argv[])
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0)
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0)
break;
}
if (i >= NUM_MISSIONS_SUPPORTED) {
@@ -173,7 +173,7 @@ int test_dataman(int argc, char *argv[])
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD, i, buffer, sizeof(buffer)) != 0) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}