From 42caba7d8ebadedbce04d756d344116c43ee325f Mon Sep 17 00:00:00 2001 From: "mich146@hotmail.com" Date: Wed, 16 Mar 2011 10:09:22 +0000 Subject: [PATCH] Mavlink update git-svn-id: https://arducopter.googlecode.com/svn/trunk@1777 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/GCS_MAVLink/README | 5 +- libraries/GCS_MAVLink/include/common/common.h | 5 +- .../GCS_MAVLink/include/common/mavlink.h | 2 +- .../include/common/mavlink_msg_auth_key.h | 104 ++++++++ .../common/mavlink_msg_global_position_int.h | 28 +- .../mavlink_msg_gps_set_global_origin.h | 42 +-- .../include/common/mavlink_msg_waypoint.h | 28 +- libraries/GCS_MAVLink/include/mavlink_types.h | 3 +- .../GCS_MAVLink/include/pixhawk/mavlink.h | 2 +- .../pixhawk/mavlink_msg_brief_feature.h | 249 ++++++++++++++++++ .../GCS_MAVLink/include/pixhawk/pixhawk.h | 3 +- .../message_definitions/common.xml | 5 + .../message_definitions/pixhawk.xml | 11 + 13 files changed, 430 insertions(+), 57 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h create mode 100644 libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h diff --git a/libraries/GCS_MAVLink/README b/libraries/GCS_MAVLink/README index c7bdf2c18e..513d5c77f7 100644 --- a/libraries/GCS_MAVLink/README +++ b/libraries/GCS_MAVLink/README @@ -2,10 +2,11 @@ MAVLink Micro Air Vehicle Message Marshalling Library This is a library for lightweight communication between Micro Air Vehicles (swarm) and/or ground control stations. - It serializes C-structs for serial channels and can be used with any type of radio modem. +For help, please visit the mailing list: http://groups.google.com/group/mavlink + MAVLink is licensed under the terms of the Lesser General Public License of the Free Software Foundation (LGPL). As MAVLink is a header-only library, compiling an application with it is considered "using the libary", not a derived work. MAVLink can therefore be used without limits in any closed-source application without publishing the source code of the closed-source application. @@ -22,7 +23,7 @@ gcc -I mavlink/include -I mavlink/include/ For more information, please visit: -http://pixhawk.ethz.ch/wiki/software/mavlink/ +http://qgroundcontrol.org/mavlink/ (c) 2009-2011 Lorenz Meier diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index 8c80fdf678..eafeec6217 100644 --- a/libraries/GCS_MAVLink/include/common/common.h +++ b/libraries/GCS_MAVLink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, February 26 2011, 13:25 UTC + * Generated on Tuesday, March 15 2011, 19:25 UTC */ #ifndef COMMON_H #define COMMON_H @@ -57,7 +57,7 @@ enum MAV_CMD MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.Servo numberPWM (microseconds, 1000 to 2000 typical)Cycle countCycle time (seconds)EmptyEmptyEmpty*/ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system.Camera ID (-1 for all)Transmission: 0: disabled, 1: enabled compressed, 2: enabled rawTransmission mode: 0: video stream, >0: single images every n seconds (decimal)Recording: 0: disabled, 1: enabled compressed, 2: enabled rawEmptyEmptyEmpty*/ MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumerationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode.Gyro calibration: 0: no, 1: yesMagnetometer calibration: 0: no, 1: yesRadio calibration: 0: no, 1: yesRadio calibration: 0: no, 1: yesEmptyEmptyEmpty*/ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode.Gyro calibration: 0: no, 1: yesMagnetometer calibration: 0: no, 1: yesGround pressure: 0: no, 1: yesRadio calibration: 0: no, 1: yesEmptyEmptyEmpty*/ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode.Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROMMission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROMReservedReservedEmptyEmptyEmpty*/ MAV_CMD_ENUM_END }; @@ -87,6 +87,7 @@ enum MAV_DATA_STREAM #include "./mavlink_msg_system_time_utc.h" #include "./mavlink_msg_change_operator_control.h" #include "./mavlink_msg_change_operator_control_ack.h" +#include "./mavlink_msg_auth_key.h" #include "./mavlink_msg_action_ack.h" #include "./mavlink_msg_action.h" #include "./mavlink_msg_set_mode.h" diff --git a/libraries/GCS_MAVLink/include/common/mavlink.h b/libraries/GCS_MAVLink/include/common/mavlink.h index 866ceed3b6..a5190a92c8 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink.h +++ b/libraries/GCS_MAVLink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, February 26 2011, 13:25 UTC + * Generated on Tuesday, March 15 2011, 19:25 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h new file mode 100644 index 0000000000..605ba7db61 --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h @@ -0,0 +1,104 @@ +// MESSAGE AUTH_KEY PACKING + +#define MAVLINK_MSG_ID_AUTH_KEY 7 + +typedef struct __mavlink_auth_key_t +{ + char key[32]; ///< key + +} mavlink_auth_key_t; + +#define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 + + +/** + * @brief Pack a auth_key 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 key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* key) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; + + i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a auth_key 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 key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* key) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; + + i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a auth_key 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 auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); +} + +/** + * @brief Send a auth_key message + * @param chan MAVLink channel to send the message + * + * @param key key + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) +{ + mavlink_message_t msg; + mavlink_msg_auth_key_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, key); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE AUTH_KEY UNPACKING + +/** + * @brief Get field key from auth_key message + * + * @return key + */ +static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char* r_data) +{ + + memcpy(r_data, msg->payload, sizeof(char)*32); + return sizeof(char)*32; +} + +/** + * @brief Decode a auth_key message into a struct + * + * @param msg The message to decode + * @param auth_key C-struct to decode the message contents into + */ +static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) +{ + mavlink_msg_auth_key_get_key(msg, auth_key->key); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h index 91a7d6c80c..1b3277a357 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h @@ -4,8 +4,8 @@ typedef struct __mavlink_global_position_int_t { - int32_t lat; ///< Latitude / X Position, expressed as * 1E7 - int32_t lon; ///< Longitude / Y Position, expressed as * 1E7 + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 @@ -21,8 +21,8 @@ typedef struct __mavlink_global_position_int_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param lat Latitude / X Position, expressed as * 1E7 - * @param lon Longitude / Y Position, expressed as * 1E7 + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 * @param alt Altitude in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 @@ -34,8 +34,8 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude / X Position, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude / Y Position, expressed as * 1E7 + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 @@ -50,8 +50,8 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @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 lat Latitude / X Position, expressed as * 1E7 - * @param lon Longitude / Y Position, expressed as * 1E7 + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 * @param alt Altitude in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 @@ -63,8 +63,8 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude / X Position, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude / Y Position, expressed as * 1E7 + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 @@ -90,8 +90,8 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @brief Send a global_position_int message * @param chan MAVLink channel to send the message * - * @param lat Latitude / X Position, expressed as * 1E7 - * @param lon Longitude / Y Position, expressed as * 1E7 + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 * @param alt Altitude in meters, expressed as * 1000 (millimeters) * @param vx Ground X Speed (Latitude), expressed as m/s * 100 * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 @@ -112,7 +112,7 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, /** * @brief Get field lat from global_position_int message * - * @return Latitude / X Position, expressed as * 1E7 + * @return Latitude, expressed as * 1E7 */ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) { @@ -127,7 +127,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_mess /** * @brief Get field lon from global_position_int message * - * @return Longitude / Y Position, expressed as * 1E7 + * @return Longitude, expressed as * 1E7 */ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h index 6f73a63b61..a4b485222c 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h @@ -6,9 +6,9 @@ typedef struct __mavlink_gps_set_global_origin_t { uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint32_t latitude; ///< global x position * 1E7 - uint32_t longitude; ///< global y position * 1E7 - uint32_t altitude; ///< global z position * 1000 + uint32_t latitude; ///< global position * 1E7 + uint32_t longitude; ///< global position * 1E7 + uint32_t altitude; ///< global position * 1000 } mavlink_gps_set_global_origin_t; @@ -22,9 +22,9 @@ typedef struct __mavlink_gps_set_global_origin_t * * @param target_system System ID * @param target_component Component ID - * @param latitude global x position * 1E7 - * @param longitude global y position * 1E7 - * @param altitude global z position * 1000 + * @param latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint32_t latitude, uint32_t longitude, uint32_t altitude) @@ -34,9 +34,9 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint32_t_by_index(latitude, i, msg->payload); // global x position * 1E7 - i += put_uint32_t_by_index(longitude, i, msg->payload); // global y position * 1E7 - i += put_uint32_t_by_index(altitude, i, msg->payload); // global z position * 1000 + i += put_uint32_t_by_index(latitude, i, msg->payload); // global position * 1E7 + i += put_uint32_t_by_index(longitude, i, msg->payload); // global position * 1E7 + i += put_uint32_t_by_index(altitude, i, msg->payload); // global position * 1000 return mavlink_finalize_message(msg, system_id, component_id, i); } @@ -49,9 +49,9 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param latitude global x position * 1E7 - * @param longitude global y position * 1E7 - * @param altitude global z position * 1000 + * @param latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_set_global_origin_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, uint32_t latitude, uint32_t longitude, uint32_t altitude) @@ -61,9 +61,9 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t syste i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint32_t_by_index(latitude, i, msg->payload); // global x position * 1E7 - i += put_uint32_t_by_index(longitude, i, msg->payload); // global y position * 1E7 - i += put_uint32_t_by_index(altitude, i, msg->payload); // global z position * 1000 + i += put_uint32_t_by_index(latitude, i, msg->payload); // global position * 1E7 + i += put_uint32_t_by_index(longitude, i, msg->payload); // global position * 1E7 + i += put_uint32_t_by_index(altitude, i, msg->payload); // global position * 1000 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); } @@ -87,9 +87,9 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i * * @param target_system System ID * @param target_component Component ID - * @param latitude global x position * 1E7 - * @param longitude global y position * 1E7 - * @param altitude global z position * 1000 + * @param latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -126,7 +126,7 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(con /** * @brief Get field latitude from gps_set_global_origin message * - * @return global x position * 1E7 + * @return global position * 1E7 */ static inline uint32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg) { @@ -141,7 +141,7 @@ static inline uint32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavl /** * @brief Get field longitude from gps_set_global_origin message * - * @return global y position * 1E7 + * @return global position * 1E7 */ static inline uint32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) { @@ -156,7 +156,7 @@ static inline uint32_t mavlink_msg_gps_set_global_origin_get_longitude(const mav /** * @brief Get field altitude from gps_set_global_origin message * - * @return global z position * 1000 + * @return global position * 1000 */ static inline uint32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h index 5619d0451c..ab1c5f836a 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h @@ -15,8 +15,8 @@ typedef struct __mavlink_waypoint_t float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - float x; ///< PARAM5 / local: x position, global: longitude - float y; ///< PARAM6 / y position: global: latitude + float x; ///< PARAM5 / local: x position, global: latitude + float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude } mavlink_waypoint_t; @@ -40,8 +40,8 @@ typedef struct __mavlink_waypoint_t * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: longitude - * @param y PARAM6 / y position: global: latitude + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude * @param z PARAM7 / z position: global: altitude * @return length of the message in bytes (excluding serial stream start sign) */ @@ -61,8 +61,8 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds i += put_float_by_index(param3, i, msg->payload); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: longitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: latitude + i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude + i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude return mavlink_finalize_message(msg, system_id, component_id, i); @@ -85,8 +85,8 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: longitude - * @param y PARAM6 / y position: global: latitude + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude * @param z PARAM7 / z position: global: altitude * @return length of the message in bytes (excluding serial stream start sign) */ @@ -106,8 +106,8 @@ static inline uint16_t mavlink_msg_waypoint_pack_chan(uint8_t system_id, uint8_t i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds i += put_float_by_index(param3, i, msg->payload); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: longitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: latitude + i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude + i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); @@ -141,8 +141,8 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: longitude - * @param y PARAM6 / y position: global: latitude + * @param x PARAM5 / local: x position, global: latitude + * @param y PARAM6 / y position: global: longitude * @param z PARAM7 / z position: global: altitude */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -293,7 +293,7 @@ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg /** * @brief Get field x from waypoint message * - * @return PARAM5 / local: x position, global: longitude + * @return PARAM5 / local: x position, global: latitude */ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) { @@ -308,7 +308,7 @@ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) /** * @brief Get field y from waypoint message * - * @return PARAM6 / y position: global: latitude + * @return PARAM6 / y position: global: longitude */ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/mavlink_types.h b/libraries/GCS_MAVLink/include/mavlink_types.h index 77f6ebf892..609cb7071f 100644 --- a/libraries/GCS_MAVLink/include/mavlink_types.h +++ b/libraries/GCS_MAVLink/include/mavlink_types.h @@ -139,7 +139,8 @@ enum MAV_FRAME { MAV_FRAME_GLOBAL = 0, MAV_FRAME_LOCAL = 1, - MAV_FRAME_MISSION = 2 + MAV_FRAME_MISSION = 2, + MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 }; #define MAVLINK_STX 0x55 ///< Packet start sign diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink.h index e7a9007c51..8a2c65c080 100644 --- a/libraries/GCS_MAVLink/include/pixhawk/mavlink.h +++ b/libraries/GCS_MAVLink/include/pixhawk/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, February 26 2011, 13:25 UTC + * Generated on Wednesday, March 2 2011, 13:12 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h new file mode 100644 index 0000000000..a61e13bcde --- /dev/null +++ b/libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_brief_feature.h @@ -0,0 +1,249 @@ +// MESSAGE BRIEF_FEATURE PACKING + +#define MAVLINK_MSG_ID_BRIEF_FEATURE 172 + +typedef struct __mavlink_brief_feature_t +{ + float x; ///< x position in m + float y; ///< y position in m + float z; ///< z position in m + uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + uint16_t size; ///< Size in pixels + uint16_t orientation; ///< Orientation + uint8_t descriptor[32]; ///< Descriptor + float response; ///< Harris operator response at this location + +} mavlink_brief_feature_t; + +#define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 + + +/** + * @brief Pack a brief_feature 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 x x position in m + * @param y y position in m + * @param z z position in m + * @param orientation_assignment Orientation assignment 0: false, 1:true + * @param size Size in pixels + * @param orientation Orientation + * @param descriptor Descriptor + * @param response Harris operator response at this location + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + + i += put_float_by_index(x, i, msg->payload); // x position in m + i += put_float_by_index(y, i, msg->payload); // y position in m + i += put_float_by_index(z, i, msg->payload); // z position in m + i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true + i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels + i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation + i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor + i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a brief_feature 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 x x position in m + * @param y y position in m + * @param z z position in m + * @param orientation_assignment Orientation assignment 0: false, 1:true + * @param size Size in pixels + * @param orientation Orientation + * @param descriptor Descriptor + * @param response Harris operator response at this location + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + + i += put_float_by_index(x, i, msg->payload); // x position in m + i += put_float_by_index(y, i, msg->payload); // y position in m + i += put_float_by_index(z, i, msg->payload); // z position in m + i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true + i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels + i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation + i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor + i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a brief_feature 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 brief_feature C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_brief_feature_t* brief_feature) +{ + return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); +} + +/** + * @brief Send a brief_feature message + * @param chan MAVLink channel to send the message + * + * @param x x position in m + * @param y y position in m + * @param z z position in m + * @param orientation_assignment Orientation assignment 0: false, 1:true + * @param size Size in pixels + * @param orientation Orientation + * @param descriptor Descriptor + * @param response Harris operator response at this location + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) +{ + mavlink_message_t msg; + mavlink_msg_brief_feature_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, orientation_assignment, size, orientation, descriptor, response); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE BRIEF_FEATURE UNPACKING + +/** + * @brief Get field x from brief_feature message + * + * @return x position in m + */ +static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload)[0]; + r.b[2] = (msg->payload)[1]; + r.b[1] = (msg->payload)[2]; + r.b[0] = (msg->payload)[3]; + return (float)r.f; +} + +/** + * @brief Get field y from brief_feature message + * + * @return y position in m + */ +static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field z from brief_feature message + * + * @return z position in m + */ +static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field orientation_assignment from brief_feature message + * + * @return Orientation assignment 0: false, 1:true + */ +static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; +} + +/** + * @brief Get field size from brief_feature message + * + * @return Size in pixels + */ +static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field orientation from brief_feature message + * + * @return Orientation + */ +static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field descriptor from brief_feature message + * + * @return Descriptor + */ +static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t* r_data) +{ + + memcpy(r_data, msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t), sizeof(uint8_t)*32); + return sizeof(uint8_t)*32; +} + +/** + * @brief Get field response from brief_feature message + * + * @return Harris operator response at this location + */ +static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[0]; + r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[1]; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[2]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[3]; + return (float)r.f; +} + +/** + * @brief Decode a brief_feature message into a struct + * + * @param msg The message to decode + * @param brief_feature C-struct to decode the message contents into + */ +static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature) +{ + brief_feature->x = mavlink_msg_brief_feature_get_x(msg); + brief_feature->y = mavlink_msg_brief_feature_get_y(msg); + brief_feature->z = mavlink_msg_brief_feature_get_z(msg); + brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); + brief_feature->size = mavlink_msg_brief_feature_get_size(msg); + brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); + mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); + brief_feature->response = mavlink_msg_brief_feature_get_response(msg); +} diff --git a/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h b/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h index 6cf2ce9201..7436b922c5 100644 --- a/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h +++ b/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, February 26 2011, 13:25 UTC + * Generated on Wednesday, March 2 2011, 13:12 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -73,6 +73,7 @@ enum DATA_TYPES #include "./mavlink_msg_point_of_interest_connection.h" #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" +#include "./mavlink_msg_brief_feature.h" #ifdef __cplusplus } #endif diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index e4aab2f541..bd912c7129 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -355,6 +355,11 @@ 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h The action id diff --git a/libraries/GCS_MAVLink/message_definitions/pixhawk.xml b/libraries/GCS_MAVLink/message_definitions/pixhawk.xml index 5b6e546aff..55e53205aa 100644 --- a/libraries/GCS_MAVLink/message_definitions/pixhawk.xml +++ b/libraries/GCS_MAVLink/message_definitions/pixhawk.xml @@ -238,5 +238,16 @@ image data bytes + + x position in m + y position in m + z position in m + Orientation assignment 0: false, 1:true + Size in pixels + Orientation + Descriptor + Harris operator response at this location + +