From d81237d43ffa0c2a917049f7d9fd97d6ce760da1 Mon Sep 17 00:00:00 2001 From: "mich146@hotmail.com" Date: Tue, 8 Feb 2011 23:16:58 +0000 Subject: [PATCH] MavLink Update git-svn-id: https://arducopter.googlecode.com/svn/trunk@1616 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/GCS_MAVLink/README | 5 +- libraries/GCS_MAVLink/include/common/common.h | 16 +- .../GCS_MAVLink/include/common/mavlink.h | 2 +- .../include/common/mavlink_msg_action_ack.h | 2 +- .../common/mavlink_msg_control_status.h | 220 ++++++++++++++ .../include/common/mavlink_msg_debug_vect.h | 60 ++-- .../common/mavlink_msg_global_position.h | 84 +++--- .../common/mavlink_msg_global_position_int.h | 42 +-- .../include/common/mavlink_msg_gps_raw.h | 98 +++--- .../mavlink_msg_nav_controller_output.h | 232 +++++++++++++++ .../common/mavlink_msg_safety_allowed_area.h | 233 +++++++++++++++ .../mavlink_msg_safety_set_allowed_area.h | 267 +++++++++++++++++ .../common/mavlink_msg_servo_output_raw.h | 244 +++++++++++++++ .../include/common/mavlink_msg_statustext.h | 8 +- .../include/common/mavlink_msg_sys_status.h | 39 +-- .../common/mavlink_msg_system_time_utc.h | 128 ++++++++ .../include/common/mavlink_msg_vfr_hud.h | 210 +++++++++++++ libraries/GCS_MAVLink/include/mavlink_types.h | 280 +++++++++--------- .../GCS_MAVLink/include/pixhawk/mavlink.h | 2 +- .../GCS_MAVLink/include/pixhawk/pixhawk.h | 3 +- libraries/GCS_MAVLink/license.txt | 165 +++++++++++ .../message_definitions/common.xml | 185 +++++++----- .../message_definitions/pixhawk.xml | 10 - 23 files changed, 2138 insertions(+), 397 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h create mode 100644 libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h create mode 100644 libraries/GCS_MAVLink/license.txt diff --git a/libraries/GCS_MAVLink/README b/libraries/GCS_MAVLink/README index c2ebb413c3..c7bdf2c18e 100644 --- a/libraries/GCS_MAVLink/README +++ b/libraries/GCS_MAVLink/README @@ -6,6 +6,9 @@ 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. +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. + To generate/update packets, select mavlink_standard_message.xml in the QGroundControl station settings view, select mavlink/include as the output directory and click on "Save and Generate". @@ -21,5 +24,5 @@ For more information, please visit: http://pixhawk.ethz.ch/wiki/software/mavlink/ -(c) 2009-2011 Lorenz Meier / PIXHAWK Team +(c) 2009-2011 Lorenz Meier diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index 57fd80e4a3..cfb2e8aa79 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 Sunday, January 23 2011, 16:32 UTC + * Generated on Tuesday, February 8 2011, 09:45 UTC */ #ifndef COMMON_H #define COMMON_H @@ -35,8 +35,9 @@ extern "C" { #include "./mavlink_msg_boot.h" #include "./mavlink_msg_system_time.h" #include "./mavlink_msg_ping.h" -#include "./mavlink_msg_action.h" +#include "./mavlink_msg_system_time_utc.h" #include "./mavlink_msg_action_ack.h" +#include "./mavlink_msg_action.h" #include "./mavlink_msg_set_mode.h" #include "./mavlink_msg_set_nav_mode.h" #include "./mavlink_msg_param_request_read.h" @@ -53,6 +54,7 @@ extern "C" { #include "./mavlink_msg_sys_status.h" #include "./mavlink_msg_rc_channels_raw.h" #include "./mavlink_msg_rc_channels_scaled.h" +#include "./mavlink_msg_servo_output_raw.h" #include "./mavlink_msg_waypoint.h" #include "./mavlink_msg_waypoint_request.h" #include "./mavlink_msg_waypoint_set_current.h" @@ -65,19 +67,21 @@ extern "C" { #include "./mavlink_msg_waypoint_set_global_reference.h" #include "./mavlink_msg_local_position_setpoint_set.h" #include "./mavlink_msg_local_position_setpoint.h" +#include "./mavlink_msg_control_status.h" +#include "./mavlink_msg_safety_set_allowed_area.h" +#include "./mavlink_msg_safety_allowed_area.h" #include "./mavlink_msg_attitude_controller_output.h" #include "./mavlink_msg_position_controller_output.h" +#include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_position_target.h" #include "./mavlink_msg_state_correction.h" #include "./mavlink_msg_set_altitude.h" #include "./mavlink_msg_request_data_stream.h" -#include "./mavlink_msg_request_dynamic_gyro_calibration.h" -#include "./mavlink_msg_request_static_calibration.h" #include "./mavlink_msg_manual_control.h" -#include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_gps_local_origin_set.h" -#include "./mavlink_msg_airspeed.h" #include "./mavlink_msg_global_position_int.h" +#include "./mavlink_msg_vfr_hud.h" +#include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_named_value_float.h" #include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_statustext.h" diff --git a/libraries/GCS_MAVLink/include/common/mavlink.h b/libraries/GCS_MAVLink/include/common/mavlink.h index 256f7bf0de..9e0ea54f65 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 Sunday, January 23 2011, 16:32 UTC + * Generated on Tuesday, February 8 2011, 09:45 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h index c519e8caca..6e93449e20 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h @@ -1,6 +1,6 @@ // MESSAGE ACTION_ACK PACKING -#define MAVLINK_MSG_ID_ACTION_ACK 62 +#define MAVLINK_MSG_ID_ACTION_ACK 9 typedef struct __mavlink_action_ack_t { diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h new file mode 100644 index 0000000000..746e889aaf --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h @@ -0,0 +1,220 @@ +// MESSAGE CONTROL_STATUS PACKING + +#define MAVLINK_MSG_ID_CONTROL_STATUS 52 + +typedef struct __mavlink_control_status_t +{ + uint8_t position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + uint8_t vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + uint8_t gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + uint8_t ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent + uint8_t control_att; ///< 0: Attitude control disabled, 1: enabled + uint8_t control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled + uint8_t control_pos_z; ///< 0: Z position control disabled, 1: enabled + uint8_t control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled + +} mavlink_control_status_t; + + + +/** + * @brief Pack a control_status 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 position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + * @param ahrs_health Attitude estimation health: 0: poor, 255: excellent + * @param control_att 0: Attitude control disabled, 1: enabled + * @param control_pos_xy 0: X, Y position control disabled, 1: enabled + * @param control_pos_z 0: Z position control disabled, 1: enabled + * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + + i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent + i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled + i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled + i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled + i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a control_status 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 position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + * @param ahrs_health Attitude estimation health: 0: poor, 255: excellent + * @param control_att 0: Attitude control disabled, 1: enabled + * @param control_pos_xy 0: X, Y position control disabled, 1: enabled + * @param control_pos_z 0: Z position control disabled, 1: enabled + * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + + i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent + i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled + i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled + i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled + i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a control_status 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 control_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_control_status_t* control_status) +{ + return mavlink_msg_control_status_pack(system_id, component_id, msg, control_status->position_fix, control_status->vision_fix, control_status->gps_fix, control_status->ahrs_health, control_status->control_att, control_status->control_pos_xy, control_status->control_pos_z, control_status->control_pos_yaw); +} + +/** + * @brief Send a control_status message + * @param chan MAVLink channel to send the message + * + * @param position_fix Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + * @param vision_fix Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + * @param gps_fix GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + * @param ahrs_health Attitude estimation health: 0: poor, 255: excellent + * @param control_att 0: Attitude control disabled, 1: enabled + * @param control_pos_xy 0: X, Y position control disabled, 1: enabled + * @param control_pos_z 0: Z position control disabled, 1: enabled + * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) +{ + mavlink_message_t msg; + mavlink_msg_control_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE CONTROL_STATUS UNPACKING + +/** + * @brief Get field position_fix from control_status message + * + * @return Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + */ +static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field vision_fix from control_status message + * + * @return Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + */ +static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field gps_fix from control_status message + * + * @return GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + */ +static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field ahrs_health from control_status message + * + * @return Attitude estimation health: 0: poor, 255: excellent + */ +static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field control_att from control_status message + * + * @return 0: Attitude control disabled, 1: enabled + */ +static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field control_pos_xy from control_status message + * + * @return 0: X, Y position control disabled, 1: enabled + */ +static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field control_pos_z from control_status message + * + * @return 0: Z position control disabled, 1: enabled + */ +static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field control_pos_yaw from control_status message + * + * @return 0: Yaw angle control disabled, 1: enabled + */ +static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Decode a control_status message into a struct + * + * @param msg The message to decode + * @param control_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_control_status_decode(const mavlink_message_t* msg, mavlink_control_status_t* control_status) +{ + control_status->position_fix = mavlink_msg_control_status_get_position_fix(msg); + control_status->vision_fix = mavlink_msg_control_status_get_vision_fix(msg); + control_status->gps_fix = mavlink_msg_control_status_get_gps_fix(msg); + control_status->ahrs_health = mavlink_msg_control_status_get_ahrs_health(msg); + control_status->control_att = mavlink_msg_control_status_get_control_att(msg); + control_status->control_pos_xy = mavlink_msg_control_status_get_control_pos_xy(msg); + control_status->control_pos_z = mavlink_msg_control_status_get_control_pos_z(msg); + control_status->control_pos_yaw = mavlink_msg_control_status_get_control_pos_yaw(msg); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h index db868a3c0d..6ea9e0558b 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h @@ -1,10 +1,10 @@ // MESSAGE DEBUG_VECT PACKING -#define MAVLINK_MSG_ID_DEBUG_VECT 70 +#define MAVLINK_MSG_ID_DEBUG_VECT 251 typedef struct __mavlink_debug_vect_t { - int8_t name[10]; ///< Name + char name[10]; ///< Name uint64_t usec; ///< Timestamp float x; ///< x float y; ///< y @@ -28,12 +28,12 @@ typedef struct __mavlink_debug_vect_t * @param z z * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const int8_t* name, uint64_t usec, float x, float y, float z) +static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) { uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - i += put_array_by_index(name, 10, i, msg->payload); // Name + i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp i += put_float_by_index(x, i, msg->payload); // x i += put_float_by_index(y, i, msg->payload); // y @@ -55,12 +55,12 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co * @param z z * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const int8_t* name, uint64_t usec, float x, float y, float z) +static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) { uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - i += put_array_by_index(name, 10, i, msg->payload); // Name + i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp i += put_float_by_index(x, i, msg->payload); // x i += put_float_by_index(y, i, msg->payload); // y @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const int8_t* name, uint64_t usec, float x, float y, float z) +static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) { mavlink_message_t msg; mavlink_msg_debug_vect_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, usec, x, y, z); @@ -109,11 +109,11 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const int * * @return Name */ -static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char* r_data) { - memcpy(r_data, msg->payload, 10); - return 10; + memcpy(r_data, msg->payload, sizeof(char)*10); + return sizeof(char)*10; } /** @@ -124,14 +124,14 @@ static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* msg) { generic_64bit r; - r.b[7] = (msg->payload+10)[0]; - r.b[6] = (msg->payload+10)[1]; - r.b[5] = (msg->payload+10)[2]; - r.b[4] = (msg->payload+10)[3]; - r.b[3] = (msg->payload+10)[4]; - r.b[2] = (msg->payload+10)[5]; - r.b[1] = (msg->payload+10)[6]; - r.b[0] = (msg->payload+10)[7]; + r.b[7] = (msg->payload+sizeof(char)*10)[0]; + r.b[6] = (msg->payload+sizeof(char)*10)[1]; + r.b[5] = (msg->payload+sizeof(char)*10)[2]; + r.b[4] = (msg->payload+sizeof(char)*10)[3]; + r.b[3] = (msg->payload+sizeof(char)*10)[4]; + r.b[2] = (msg->payload+sizeof(char)*10)[5]; + r.b[1] = (msg->payload+sizeof(char)*10)[6]; + r.b[0] = (msg->payload+sizeof(char)*10)[7]; return (uint64_t)r.ll; } @@ -143,10 +143,10 @@ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) { generic_32bit r; - r.b[3] = (msg->payload+10+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+10+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+10+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+10+sizeof(uint64_t))[3]; + r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[0]; + r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[1]; + r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[2]; + r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[3]; return (float)r.f; } @@ -158,10 +158,10 @@ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) { generic_32bit r; - r.b[3] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+10+sizeof(uint64_t)+sizeof(float))[3]; + r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[3]; return (float)r.f; } @@ -173,10 +173,10 @@ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) { generic_32bit r; - r.b[3] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; + r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; return (float)r.f; } diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h index a6b5cd729e..9d27f7714b 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h @@ -5,12 +5,12 @@ typedef struct __mavlink_global_position_t { uint64_t usec; ///< Timestamp (microseconds since unix epoch) - float lat; ///< X Position - float lon; ///< Y Position - float alt; ///< Z Position - float vx; ///< X Speed - float vy; ///< Y Speed - float vz; ///< Z Speed + float lat; ///< Latitude, in degrees + float lon; ///< Longitude, in degrees + float alt; ///< Absolute altitude, in meters + float vx; ///< X Speed (in Latitude direction, positive: going north) + float vy; ///< Y Speed (in Longitude direction, positive: going east) + float vz; ///< Z Speed (in Altitude direction, positive: going up) } mavlink_global_position_t; @@ -23,12 +23,12 @@ typedef struct __mavlink_global_position_t * @param msg The MAVLink message to compress the data into * * @param usec Timestamp (microseconds since unix epoch) - * @param lat X Position - * @param lon Y Position - * @param alt Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) @@ -37,12 +37,12 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // X Position - i += put_float_by_index(lon, i, msg->payload); // Y Position - i += put_float_by_index(alt, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed + i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees + i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees + i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters + i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) + i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) + i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) return mavlink_finalize_message(msg, system_id, component_id, i); } @@ -54,12 +54,12 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 * @param chan The MAVLink channel this message was sent over * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds since unix epoch) - * @param lat X Position - * @param lon Y Position - * @param alt Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) @@ -68,12 +68,12 @@ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // X Position - i += put_float_by_index(lon, i, msg->payload); // Y Position - i += put_float_by_index(alt, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed + i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees + i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees + i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters + i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) + i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) + i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); } @@ -96,12 +96,12 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin * @param chan MAVLink channel to send the message * * @param usec Timestamp (microseconds since unix epoch) - * @param lat X Position - * @param lon Y Position - * @param alt Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -137,7 +137,7 @@ static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_messag /** * @brief Get field lat from global_position message * - * @return X Position + * @return Latitude, in degrees */ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* msg) { @@ -152,7 +152,7 @@ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* /** * @brief Get field lon from global_position message * - * @return Y Position + * @return Longitude, in degrees */ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* msg) { @@ -167,7 +167,7 @@ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* /** * @brief Get field alt from global_position message * - * @return Z Position + * @return Absolute altitude, in meters */ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* msg) { @@ -182,7 +182,7 @@ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* /** * @brief Get field vx from global_position message * - * @return X Speed + * @return X Speed (in Latitude direction, positive: going north) */ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* msg) { @@ -197,7 +197,7 @@ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* /** * @brief Get field vy from global_position message * - * @return Y Speed + * @return Y Speed (in Longitude direction, positive: going east) */ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* msg) { @@ -212,7 +212,7 @@ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* /** * @brief Get field vz from global_position message * - * @return Z Speed + * @return Z Speed (in Altitude direction, positive: going up) */ static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* msg) { 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 6c202e6623..c58ac22d98 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 @@ -7,9 +7,9 @@ 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 alt; ///< Altitude / negative Z Position, expressed as * 1000 - int16_t vx; ///< Ground X Speed, expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed, expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed, expressed as m/s * 100 + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 } mavlink_global_position_int_t; @@ -24,9 +24,9 @@ typedef struct __mavlink_global_position_int_t * @param lat Latitude / X Position, expressed as * 1E7 * @param lon Longitude / Y Position, expressed as * 1E7 * @param alt Altitude / negative Z Position, expressed as * 1000 - * @param vx Ground X Speed, expressed as m/s * 100 - * @param vy Ground Y Speed, expressed as m/s * 100 - * @param vz Ground Z Speed, expressed as m/s * 100 + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) @@ -37,9 +37,9 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u 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(alt, i, msg->payload); // Altitude / negative Z Position, expressed as * 1000 - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed, expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed, expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed, expressed as m/s * 100 + 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 + i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 return mavlink_finalize_message(msg, system_id, component_id, i); } @@ -53,9 +53,9 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @param lat Latitude / X Position, expressed as * 1E7 * @param lon Longitude / Y Position, expressed as * 1E7 * @param alt Altitude / negative Z Position, expressed as * 1000 - * @param vx Ground X Speed, expressed as m/s * 100 - * @param vy Ground Y Speed, expressed as m/s * 100 - * @param vz Ground Z Speed, expressed as m/s * 100 + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) @@ -66,9 +66,9 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ 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(alt, i, msg->payload); // Altitude / negative Z Position, expressed as * 1000 - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed, expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed, expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed, expressed as m/s * 100 + 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 + i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); } @@ -93,9 +93,9 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param lat Latitude / X Position, expressed as * 1E7 * @param lon Longitude / Y Position, expressed as * 1E7 * @param alt Altitude / negative Z Position, expressed as * 1000 - * @param vx Ground X Speed, expressed as m/s * 100 - * @param vy Ground Y Speed, expressed as m/s * 100 - * @param vz Ground Z Speed, expressed as m/s * 100 + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -157,7 +157,7 @@ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_mess /** * @brief Get field vx from global_position_int message * - * @return Ground X Speed, expressed as m/s * 100 + * @return Ground X Speed (Latitude), expressed as m/s * 100 */ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) { @@ -170,7 +170,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_messa /** * @brief Get field vy from global_position_int message * - * @return Ground Y Speed, expressed as m/s * 100 + * @return Ground Y Speed (Longitude), expressed as m/s * 100 */ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) { @@ -183,7 +183,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_messa /** * @brief Get field vz from global_position_int message * - * @return Ground Z Speed, expressed as m/s * 100 + * @return Ground Z Speed (Altitude), expressed as m/s * 100 */ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h index 930e0ac0c7..d6264b5533 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h @@ -6,13 +6,13 @@ typedef struct __mavlink_gps_raw_t { uint64_t usec; ///< Timestamp (microseconds since unix epoch) uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix - float lat; ///< X Position - float lon; ///< Y Position - float alt; ///< Z Position in meters - float eph; ///< Uncertainty in meters of latitude - float epv; ///< Uncertainty in meters of longitude - float v; ///< Overall speed - float hdg; ///< Heading, in FIXME + float lat; ///< Latitude in degrees + float lon; ///< Longitude in degrees + float alt; ///< Altitude in meters + float eph; ///< GPS HDOP + float epv; ///< GPS VDOP + float v; ///< GPS ground speed + float hdg; ///< Compass heading in degrees, 0..360 degrees } mavlink_gps_raw_t; @@ -26,13 +26,13 @@ typedef struct __mavlink_gps_raw_t * * @param usec Timestamp (microseconds since unix epoch) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix - * @param lat X Position - * @param lon Y Position - * @param alt Z Position in meters - * @param eph Uncertainty in meters of latitude - * @param epv Uncertainty in meters of longitude - * @param v Overall speed - * @param hdg Heading, in FIXME + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed + * @param hdg Compass heading in degrees, 0..360 degrees * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) @@ -42,13 +42,13 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) i += put_uint8_t_by_index(fix_type, i, msg->payload); // 0-1: no fix, 2: 2D fix, 3: 3D fix - i += put_float_by_index(lat, i, msg->payload); // X Position - i += put_float_by_index(lon, i, msg->payload); // Y Position - i += put_float_by_index(alt, i, msg->payload); // Z Position in meters - i += put_float_by_index(eph, i, msg->payload); // Uncertainty in meters of latitude - i += put_float_by_index(epv, i, msg->payload); // Uncertainty in meters of longitude - i += put_float_by_index(v, i, msg->payload); // Overall speed - i += put_float_by_index(hdg, i, msg->payload); // Heading, in FIXME + i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees + i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees + i += put_float_by_index(alt, i, msg->payload); // Altitude in meters + i += put_float_by_index(eph, i, msg->payload); // GPS HDOP + i += put_float_by_index(epv, i, msg->payload); // GPS VDOP + i += put_float_by_index(v, i, msg->payload); // GPS ground speed + i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees return mavlink_finalize_message(msg, system_id, component_id, i); } @@ -61,13 +61,13 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo * @param msg The MAVLink message to compress the data into * @param usec Timestamp (microseconds since unix epoch) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix - * @param lat X Position - * @param lon Y Position - * @param alt Z Position in meters - * @param eph Uncertainty in meters of latitude - * @param epv Uncertainty in meters of longitude - * @param v Overall speed - * @param hdg Heading, in FIXME + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed + * @param hdg Compass heading in degrees, 0..360 degrees * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) @@ -77,13 +77,13 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) i += put_uint8_t_by_index(fix_type, i, msg->payload); // 0-1: no fix, 2: 2D fix, 3: 3D fix - i += put_float_by_index(lat, i, msg->payload); // X Position - i += put_float_by_index(lon, i, msg->payload); // Y Position - i += put_float_by_index(alt, i, msg->payload); // Z Position in meters - i += put_float_by_index(eph, i, msg->payload); // Uncertainty in meters of latitude - i += put_float_by_index(epv, i, msg->payload); // Uncertainty in meters of longitude - i += put_float_by_index(v, i, msg->payload); // Overall speed - i += put_float_by_index(hdg, i, msg->payload); // Heading, in FIXME + i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees + i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees + i += put_float_by_index(alt, i, msg->payload); // Altitude in meters + i += put_float_by_index(eph, i, msg->payload); // GPS HDOP + i += put_float_by_index(epv, i, msg->payload); // GPS VDOP + i += put_float_by_index(v, i, msg->payload); // GPS ground speed + i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); } @@ -107,13 +107,13 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * * @param usec Timestamp (microseconds since unix epoch) * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix - * @param lat X Position - * @param lon Y Position - * @param alt Z Position in meters - * @param eph Uncertainty in meters of latitude - * @param epv Uncertainty in meters of longitude - * @param v Overall speed - * @param hdg Heading, in FIXME + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed + * @param hdg Compass heading in degrees, 0..360 degrees */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -159,7 +159,7 @@ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* /** * @brief Get field lat from gps_raw message * - * @return X Position + * @return Latitude in degrees */ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) { @@ -174,7 +174,7 @@ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) /** * @brief Get field lon from gps_raw message * - * @return Y Position + * @return Longitude in degrees */ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) { @@ -189,7 +189,7 @@ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) /** * @brief Get field alt from gps_raw message * - * @return Z Position in meters + * @return Altitude in meters */ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) { @@ -204,7 +204,7 @@ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from gps_raw message * - * @return Uncertainty in meters of latitude + * @return GPS HDOP */ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) { @@ -219,7 +219,7 @@ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) /** * @brief Get field epv from gps_raw message * - * @return Uncertainty in meters of longitude + * @return GPS VDOP */ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) { @@ -234,7 +234,7 @@ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) /** * @brief Get field v from gps_raw message * - * @return Overall speed + * @return GPS ground speed */ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) { @@ -249,7 +249,7 @@ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) /** * @brief Get field hdg from gps_raw message * - * @return Heading, in FIXME + * @return Compass heading in degrees, 0..360 degrees */ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h new file mode 100644 index 0000000000..f348c523da --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h @@ -0,0 +1,232 @@ +// MESSAGE NAV_CONTROLLER_OUTPUT PACKING + +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 + +typedef struct __mavlink_nav_controller_output_t +{ + float nav_roll; ///< Current desired roll in degrees + float nav_pitch; ///< Current desired pitch in degrees + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current waypoint/target in degrees + uint16_t wp_dist; ///< Distance to active waypoint in meters + float alt_error; ///< Current altitude error in meters + float aspd_error; ///< Current airspeed error in meters/second + +} mavlink_nav_controller_output_t; + + + +/** + * @brief Pack a nav_controller_output 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 nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + + i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees + i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees + i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees + i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees + i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters + i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters + i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a nav_controller_output 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 nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + + i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees + i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees + i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees + i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees + i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters + i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters + i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a nav_controller_output 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 nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error); +} + +/** + * @brief Send a nav_controller_output message + * @param chan MAVLink channel to send the message + * + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error) +{ + mavlink_message_t msg; + mavlink_msg_nav_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING + +/** + * @brief Get field nav_roll from nav_controller_output message + * + * @return Current desired roll in degrees + */ +static inline float mavlink_msg_nav_controller_output_get_nav_roll(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 nav_pitch from nav_controller_output message + * + * @return Current desired pitch in degrees + */ +static inline float mavlink_msg_nav_controller_output_get_nav_pitch(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 nav_bearing from nav_controller_output message + * + * @return Current desired heading in degrees + */ +static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field target_bearing from nav_controller_output message + * + * @return Bearing to current waypoint/target in degrees + */ +static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field wp_dist from nav_controller_output message + * + * @return Distance to active waypoint in meters + */ +static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field alt_error from nav_controller_output message + * + * @return Current altitude error in meters + */ +static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[0]; + r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[1]; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[2]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field aspd_error from nav_controller_output message + * + * @return Current airspeed error in meters/second + */ +static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a nav_controller_output message into a struct + * + * @param msg The message to decode + * @param nav_controller_output C-struct to decode the message contents into + */ +static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output) +{ + nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); + nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); + nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); + nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); + nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); + nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); + nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h new file mode 100644 index 0000000000..e1f9cc6b12 --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h @@ -0,0 +1,233 @@ +// MESSAGE SAFETY_ALLOWED_AREA PACKING + +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 + +typedef struct __mavlink_safety_allowed_area_t +{ + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 + +} mavlink_safety_allowed_area_t; + + + +/** + * @brief Pack a safety_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + + i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 + i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 + i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 + i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 + i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 + i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a safety_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + + i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 + i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 + i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 + i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 + i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 + i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a safety_allowed_area 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 safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + +/** + * @brief Send a safety_allowed_area message + * @param chan MAVLink channel to send the message + * + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ + mavlink_message_t msg; + mavlink_msg_safety_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SAFETY_ALLOWED_AREA UNPACKING + +/** + * @brief Get field frame from safety_allowed_area message + * + * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + */ +static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field p1x from safety_allowed_area message + * + * @return x position 1 / Latitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field p1y from safety_allowed_area message + * + * @return y position 1 / Longitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p1z from safety_allowed_area message + * + * @return z position 1 / Altitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p2x from safety_allowed_area message + * + * @return x position 2 / Latitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p2y from safety_allowed_area message + * + * @return y position 2 / Longitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p2z from safety_allowed_area message + * + * @return z position 2 / Altitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a safety_allowed_area message into a struct + * + * @param msg The message to decode + * @param safety_allowed_area C-struct to decode the message contents into + */ +static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area) +{ + safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); + safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); + safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); + safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); + safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); + safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); + safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h new file mode 100644 index 0000000000..da571e8e72 --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h @@ -0,0 +1,267 @@ +// MESSAGE SAFETY_SET_ALLOWED_AREA PACKING + +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 + +typedef struct __mavlink_safety_set_allowed_area_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 + +} mavlink_safety_set_allowed_area_t; + + + +/** + * @brief Pack a safety_set_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + + 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_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 + i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 + i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 + i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 + i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 + i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a safety_set_allowed_area 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 target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_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 frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + + 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_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 + i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 + i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 + i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 + i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 + i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a safety_set_allowed_area 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 safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + +/** + * @brief Send a safety_set_allowed_area message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ + mavlink_message_t msg; + mavlink_msg_safety_set_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING + +/** + * @brief Get field target_system from safety_set_allowed_area message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload)[0]; +} + +/** + * @brief Get field target_component from safety_set_allowed_area message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field frame from safety_set_allowed_area message + * + * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) +{ + return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; +} + +/** + * @brief Get field p1x from safety_set_allowed_area message + * + * @return x position 1 / Latitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field p1y from safety_set_allowed_area message + * + * @return y position 1 / Longitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p1z from safety_set_allowed_area message + * + * @return z position 1 / Altitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p2x from safety_set_allowed_area message + * + * @return x position 2 / Latitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p2y from safety_set_allowed_area message + * + * @return y position 2 / Longitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Get field p2z from safety_set_allowed_area message + * + * @return z position 2 / Altitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; + return (float)r.f; +} + +/** + * @brief Decode a safety_set_allowed_area message into a struct + * + * @param msg The message to decode + * @param safety_set_allowed_area C-struct to decode the message contents into + */ +static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); + safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); + safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); + safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); + safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); + safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); + safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); + safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); + safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h new file mode 100644 index 0000000000..751afcb80a --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h @@ -0,0 +1,244 @@ +// MESSAGE SERVO_OUTPUT_RAW PACKING + +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 + +typedef struct __mavlink_servo_output_raw_t +{ + uint16_t servo1_raw; ///< Servo output 1 value, in microseconds + uint16_t servo2_raw; ///< Servo output 2 value, in microseconds + uint16_t servo3_raw; ///< Servo output 3 value, in microseconds + uint16_t servo4_raw; ///< Servo output 4 value, in microseconds + uint16_t servo5_raw; ///< Servo output 5 value, in microseconds + uint16_t servo6_raw; ///< Servo output 6 value, in microseconds + uint16_t servo7_raw; ///< Servo output 7 value, in microseconds + uint16_t servo8_raw; ///< Servo output 8 value, in microseconds + +} mavlink_servo_output_raw_t; + + + +/** + * @brief Pack a servo_output_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 servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + + i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds + i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds + i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds + i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds + i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds + i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds + i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds + i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a servo_output_raw 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 servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + + i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds + i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds + i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds + i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds + i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds + i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds + i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds + i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a servo_output_raw 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 servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + +/** + * @brief Send a servo_output_raw message + * @param chan MAVLink channel to send the message + * + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ + mavlink_message_t msg; + mavlink_msg_servo_output_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SERVO_OUTPUT_RAW UNPACKING + +/** + * @brief Get field servo1_raw from servo_output_raw message + * + * @return Servo output 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload)[0]; + r.b[0] = (msg->payload)[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo2_raw from servo_output_raw message + * + * @return Servo output 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo3_raw from servo_output_raw message + * + * @return Servo output 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo4_raw from servo_output_raw message + * + * @return Servo output 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo5_raw from servo_output_raw message + * + * @return Servo output 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo6_raw from servo_output_raw message + * + * @return Servo output 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo7_raw from servo_output_raw message + * + * @return Servo output 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field servo8_raw from servo_output_raw message + * + * @return Servo output 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Decode a servo_output_raw message into a struct + * + * @param msg The message to decode + * @param servo_output_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw) +{ + servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); + servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); + servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); + servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); + servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); + servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); + servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); + servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h index c3b305e549..681b659c4d 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h @@ -28,7 +28,7 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index(text, 50, i, msg->payload); // Status text message, without null termination character + i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character return mavlink_finalize_message(msg, system_id, component_id, i); } @@ -49,7 +49,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index(text, 50, i, msg->payload); // Status text message, without null termination character + i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); } @@ -104,8 +104,8 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t* r_data) { - memcpy(r_data, msg->payload+sizeof(uint8_t), 50); - return 50; + memcpy(r_data, msg->payload+sizeof(uint8_t), sizeof(int8_t)*50); + return sizeof(int8_t)*50; } /** diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h index d39ff7956c..ef83d84481 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h @@ -9,7 +9,7 @@ typedef struct __mavlink_sys_status_t uint8_t status; ///< System status flag, see MAV_STATUS ENUM uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) - uint8_t motor_block; ///< Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) } mavlink_sys_status_t; @@ -27,11 +27,11 @@ typedef struct __mavlink_sys_status_t * @param status System status flag, see MAV_STATUS ENUM * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) - * @param motor_block Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint8_t motor_block, uint16_t packet_drop) +static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; @@ -41,7 +41,7 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint8_t_by_index(motor_block, i, msg->payload); // Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) return mavlink_finalize_message(msg, system_id, component_id, i); @@ -58,11 +58,11 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @param status System status flag, see MAV_STATUS ENUM * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) - * @param motor_block Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint8_t motor_block, uint16_t packet_drop) +static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; @@ -72,7 +72,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint8_t_by_index(motor_block, i, msg->payload); // Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); @@ -88,7 +88,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) { - return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->mode, sys_status->nav_mode, sys_status->status, sys_status->load, sys_status->vbat, sys_status->motor_block, sys_status->packet_drop); + return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->mode, sys_status->nav_mode, sys_status->status, sys_status->load, sys_status->vbat, sys_status->battery_remaining, sys_status->packet_drop); } /** @@ -100,15 +100,15 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param status System status flag, see MAV_STATUS ENUM * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) - * @param motor_block Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint8_t motor_block, uint16_t packet_drop) +static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_message_t msg; - mavlink_msg_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, status, load, vbat, motor_block, packet_drop); + mavlink_msg_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop); mavlink_send_uart(chan, &msg); } @@ -172,13 +172,16 @@ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* } /** - * @brief Get field motor_block from sys_status message + * @brief Get field battery_remaining from sys_status message * - * @return Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + * @return Remaining battery energy: (0%: 0, 100%: 1000) */ -static inline uint8_t mavlink_msg_sys_status_get_motor_block(const mavlink_message_t* msg) +static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + generic_16bit r; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; + return (uint16_t)r.s; } /** @@ -189,8 +192,8 @@ static inline uint8_t mavlink_msg_sys_status_get_motor_block(const mavlink_messa static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_message_t* msg) { generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[1]; + r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; return (uint16_t)r.s; } @@ -207,6 +210,6 @@ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, m sys_status->status = mavlink_msg_sys_status_get_status(msg); sys_status->load = mavlink_msg_sys_status_get_load(msg); sys_status->vbat = mavlink_msg_sys_status_get_vbat(msg); - sys_status->motor_block = mavlink_msg_sys_status_get_motor_block(msg); + sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); sys_status->packet_drop = mavlink_msg_sys_status_get_packet_drop(msg); } diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h new file mode 100644 index 0000000000..75c25f1ced --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h @@ -0,0 +1,128 @@ +// MESSAGE SYSTEM_TIME_UTC PACKING + +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 + +typedef struct __mavlink_system_time_utc_t +{ + uint32_t utc_date; ///< GPS UTC date ddmmyy + uint32_t utc_time; ///< GPS UTC time hhmmss + +} mavlink_system_time_utc_t; + + + +/** + * @brief Pack a system_time_utc 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 utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t utc_date, uint32_t utc_time) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + + i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy + i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a system_time_utc 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 utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t utc_date, uint32_t utc_time) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + + i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy + i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a system_time_utc 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 system_time_utc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_utc_t* system_time_utc) +{ + return mavlink_msg_system_time_utc_pack(system_id, component_id, msg, system_time_utc->utc_date, system_time_utc->utc_time); +} + +/** + * @brief Send a system_time_utc message + * @param chan MAVLink channel to send the message + * + * @param utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) +{ + mavlink_message_t msg; + mavlink_msg_system_time_utc_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, utc_date, utc_time); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE SYSTEM_TIME_UTC UNPACKING + +/** + * @brief Get field utc_date from system_time_utc message + * + * @return GPS UTC date ddmmyy + */ +static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(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 (uint32_t)r.i; +} + +/** + * @brief Get field utc_time from system_time_utc message + * + * @return GPS UTC time hhmmss + */ +static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(uint32_t))[0]; + r.b[2] = (msg->payload+sizeof(uint32_t))[1]; + r.b[1] = (msg->payload+sizeof(uint32_t))[2]; + r.b[0] = (msg->payload+sizeof(uint32_t))[3]; + return (uint32_t)r.i; +} + +/** + * @brief Decode a system_time_utc message into a struct + * + * @param msg The message to decode + * @param system_time_utc C-struct to decode the message contents into + */ +static inline void mavlink_msg_system_time_utc_decode(const mavlink_message_t* msg, mavlink_system_time_utc_t* system_time_utc) +{ + system_time_utc->utc_date = mavlink_msg_system_time_utc_get_utc_date(msg); + system_time_utc->utc_time = mavlink_msg_system_time_utc_get_utc_time(msg); +} diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h new file mode 100644 index 0000000000..e64ca2f34f --- /dev/null +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h @@ -0,0 +1,210 @@ +// MESSAGE VFR_HUD PACKING + +#define MAVLINK_MSG_ID_VFR_HUD 74 + +typedef struct __mavlink_vfr_hud_t +{ + float airspeed; ///< Current airspeed in m/s + float groundspeed; ///< Current ground speed in m/s + int16_t heading; ///< Current heading in degrees + uint16_t alt; ///< Current altitude (MSL), in meters + float climb; ///< Current climb rate in meters/second + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 + +} mavlink_vfr_hud_t; + + + +/** + * @brief Pack a vfr_hud 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 airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float airspeed, float groundspeed, int16_t heading, uint16_t alt, float climb, uint16_t throttle) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; + + i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s + i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s + i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees + i += put_uint16_t_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters + i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second + i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 + + return mavlink_finalize_message(msg, system_id, component_id, i); +} + +/** + * @brief Pack a vfr_hud 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 airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float airspeed, float groundspeed, int16_t heading, uint16_t alt, float climb, uint16_t throttle) +{ + uint16_t i = 0; + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; + + i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s + i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s + i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees + i += put_uint16_t_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters + i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second + i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); +} + +/** + * @brief Encode a vfr_hud 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 vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->alt, vfr_hud->climb, vfr_hud->throttle); +} + +/** + * @brief Send a vfr_hud message + * @param chan MAVLink channel to send the message + * + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @param throttle Current throttle setting in integer percent, 0 to 100 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t alt, float climb, uint16_t throttle) +{ + mavlink_message_t msg; + mavlink_msg_vfr_hud_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, airspeed, groundspeed, heading, alt, climb, throttle); + mavlink_send_uart(chan, &msg); +} + +#endif +// MESSAGE VFR_HUD UNPACKING + +/** + * @brief Get field airspeed from vfr_hud message + * + * @return Current airspeed in m/s + */ +static inline float mavlink_msg_vfr_hud_get_airspeed(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 groundspeed from vfr_hud message + * + * @return Current ground speed in m/s + */ +static inline float mavlink_msg_vfr_hud_get_groundspeed(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 heading from vfr_hud message + * + * @return Current heading in degrees + */ +static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; + return (int16_t)r.s; +} + +/** + * @brief Get field alt from vfr_hud message + * + * @return Current altitude (MSL), in meters + */ +static inline uint16_t mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Get field climb from vfr_hud message + * + * @return Current climb rate in meters/second + */ +static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[0]; + r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[1]; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[2]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[3]; + return (float)r.f; +} + +/** + * @brief Get field throttle from vfr_hud message + * + * @return Current throttle setting in integer percent, 0 to 100 + */ +static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) +{ + generic_16bit r; + r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; + r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; + return (uint16_t)r.s; +} + +/** + * @brief Decode a vfr_hud message into a struct + * + * @param msg The message to decode + * @param vfr_hud C-struct to decode the message contents into + */ +static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud) +{ + vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); + vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); + vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); + vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); + vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); + vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); +} diff --git a/libraries/GCS_MAVLink/include/mavlink_types.h b/libraries/GCS_MAVLink/include/mavlink_types.h index 34d3d6c311..3665819f15 100644 --- a/libraries/GCS_MAVLink/include/mavlink_types.h +++ b/libraries/GCS_MAVLink/include/mavlink_types.h @@ -3,100 +3,107 @@ #include "inttypes.h" -enum MAV_CLASS { - MAV_CLASS_GENERIC = 0, - MAV_CLASS_PIXHAWK = 1, - MAV_CLASS_SLUGS = 2, - MAV_CLASS_ARDUPILOTMEGA = 3 +enum MAV_CLASS +{ + MAV_CLASS_GENERIC = 0, + MAV_CLASS_PIXHAWK = 1, + MAV_CLASS_SLUGS = 2, + MAV_CLASS_ARDUPILOTMEGA = 3 }; -enum MAV_ACTION { - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACITON_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_NB ///< Number of MAV actions +enum MAV_ACTION +{ + MAV_ACTION_HOLD = 0, + MAV_ACTION_MOTORS_START = 1, + MAV_ACTION_LAUNCH = 2, + MAV_ACTION_RETURN = 3, + MAV_ACTION_EMCY_LAND = 4, + MAV_ACTION_EMCY_KILL = 5, + MAV_ACTION_CONFIRM_KILL = 6, + MAV_ACTION_CONTINUE = 7, + MAV_ACTION_MOTORS_STOP = 8, + MAV_ACTION_HALT = 9, + MAV_ACTION_SHUTDOWN = 10, + MAV_ACTION_REBOOT = 11, + MAV_ACTION_SET_MANUAL = 12, + MAV_ACTION_SET_AUTO = 13, + MAV_ACTION_STORAGE_READ = 14, + MAV_ACTION_STORAGE_WRITE = 15, + MAV_ACTION_CALIBRATE_RC = 16, + MAV_ACTION_CALIBRATE_GYRO = 17, + MAV_ACTION_CALIBRATE_MAG = 18, + MAV_ACTION_CALIBRATE_ACC = 19, + MAV_ACTION_CALIBRATE_PRESSURE = 20, + MAV_ACTION_REC_START = 21, + MAV_ACTION_REC_PAUSE = 22, + MAV_ACTION_REC_STOP = 23, + MAV_ACTION_TAKEOFF = 24, + MAV_ACTION_NAVIGATE = 25, + MAV_ACTION_LAND = 26, + MAV_ACTION_LOITER = 27, + MAV_ACTION_SET_ORIGIN = 28, + MAV_ACTION_RELAY_ON = 29, + MAV_ACTION_RELAY_OFF = 30, + MAV_ACTION_GET_IMAGE = 31, + MAV_ACTION_VIDEO_START = 32, + MAV_ACTION_VIDEO_STOP = 33, + MAV_ACTION_RESET_MAP = 34, + MAV_ACTION_RESET_PLAN = 35, + MAV_ACTION_DELAY_BEFORE_COMMAND = 36, + MAV_ACTION_ASCEND_AT_RATE = 37, + MAV_ACTION_CHANGE_MODE = 38, + MAV_ACTION_LOITER_MAX_TURNS = 39, + MAV_ACTION_LOITER_MAX_TIME = 40, + MAV_ACTION_NB ///< Number of MAV actions }; enum MAV_MODE { - MAV_MODE_UNINIT = 0, - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back + MAV_MODE_UNINIT = 0, ///< System is in undefined state + MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe + MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control + MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint + MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation + MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use + MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use + MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use + MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive + MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back }; enum MAV_STATE { - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_POWEROFF + MAV_STATE_UNINIT = 0, + MAV_STATE_BOOT, + MAV_STATE_CALIBRATING, + MAV_STATE_STANDBY, + MAV_STATE_ACTIVE, + MAV_STATE_CRITICAL, + MAV_STATE_EMERGENCY, + MAV_STATE_POWEROFF }; enum MAV_NAV { - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST + MAV_NAV_GROUNDED = 0, + MAV_NAV_LIFTOFF, + MAV_NAV_HOLD, + MAV_NAV_WAYPOINT, + MAV_NAV_VECTOR, + MAV_NAV_RETURNING, + MAV_NAV_LANDING, + MAV_NAV_LOST }; enum MAV_TYPE { - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6 + MAV_GENERIC = 0, + MAV_FIXED_WING = 1, + MAV_QUADROTOR = 2, + MAV_COAXIAL = 3, + MAV_HELICOPTER = 4, + MAV_GROUND = 5, + OCU = 6 }; enum MAV_AUTOPILOT_TYPE @@ -105,28 +112,30 @@ enum MAV_AUTOPILOT_TYPE MAV_AUTOPILOT_PIXHAWK = 1, MAV_AUTOPILOT_SLUGS = 2, MAV_AUTOPILOT_ARDUPILOTMEGA = 3 -}; + }; enum MAV_COMPONENT { - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_UDP_BRIDGE = 240, - MAV_COMP_ID_UART_BRIDGE = 241, - MAV_COMP_ID_SYSTEM_CONTROL = 250 -}; + MAV_COMP_ID_GPS, + MAV_COMP_ID_WAYPOINTPLANNER, + MAV_COMP_ID_BLOBTRACKER, + MAV_COMP_ID_PATHPLANNER, + MAV_COMP_ID_AIRSLAM, + MAV_COMP_ID_MAPPER, + MAV_COMP_ID_IMU = 200, + MAV_COMP_ID_UDP_BRIDGE = 240, + MAV_COMP_ID_UART_BRIDGE = 241, + MAV_COMP_ID_SYSTEM_CONTROL = 250 + }; enum MAV_FRAME { MAV_FRAME_GLOBAL = 0, - MAV_FRAME_LOCAL = 1 + MAV_FRAME_LOCAL = 1, + MAV_FRAME_MISSION = 2 }; -enum MAV_DATA_STREAM{ +enum MAV_DATA_STREAM +{ MAV_DATA_STREAM_ALL = 0, MAV_DATA_STREAM_RAW_SENSORS = 1, MAV_DATA_STREAM_EXTENDED_STATUS = 2, @@ -141,10 +150,11 @@ enum MAV_DATA_STREAM{ -enum DATA_TYPES { - DATA_TYPE_JPEG_IMAGE = 0, - DATA_TYPE_RAW_IMAGE = 1, - DATA_TYPE_KINECT +enum DATA_TYPES +{ + DATA_TYPE_JPEG_IMAGE = 0, + DATA_TYPE_RAW_IMAGE = 1, + DATA_TYPE_KINECT }; #define MAVLINK_STX 0x55 ///< Packet start sign @@ -161,58 +171,58 @@ enum DATA_TYPES { //#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - 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 sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + 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 } mavlink_system_t; typedef struct __mavlink_message { - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU + uint8_t ck_a; ///< Checksum high byte + uint8_t ck_b; ///< Checksum low byte } mavlink_message_t; typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_NB, - MAVLINK_COMM_NB_HIGH = 16 -} mavlink_channel_t; + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 + } mavlink_channel_t; typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 } mavlink_parse_state_t; ///< The state machine for the comm parser typedef struct __mavlink_status { - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops + uint8_t ck_a; ///< Checksum byte 1 + uint8_t ck_b; ///< Checksum byte 2 + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops } mavlink_status_t; #endif /* MAVLINK_TYPES_H_ */ diff --git a/libraries/GCS_MAVLink/include/pixhawk/mavlink.h b/libraries/GCS_MAVLink/include/pixhawk/mavlink.h index 7f6f9a043d..83c7784ae6 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 Friday, January 14 2011, 17:41 UTC + * Generated on Tuesday, February 8 2011, 09:45 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h b/libraries/GCS_MAVLink/include/pixhawk/pixhawk.h index 8c04bd0322..0cd64b5c95 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 Friday, January 14 2011, 17:41 UTC + * Generated on Tuesday, February 8 2011, 09:45 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -54,7 +54,6 @@ enum SLUGS_PID_INDX_IDS #include "./mavlink_msg_marker.h" #include "./mavlink_msg_raw_aux.h" #include "./mavlink_msg_aux_status.h" -#include "./mavlink_msg_control_status.h" #include "./mavlink_msg_watchdog_heartbeat.h" #include "./mavlink_msg_watchdog_process_info.h" #include "./mavlink_msg_watchdog_process_status.h" diff --git a/libraries/GCS_MAVLink/license.txt b/libraries/GCS_MAVLink/license.txt new file mode 100644 index 0000000000..65c5ca88a6 --- /dev/null +++ b/libraries/GCS_MAVLink/license.txt @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index 1a1cbecabb..673e9f60ea 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -26,6 +26,18 @@ 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system Unix timestamp in microseconds + + + UTC date and time from GPS module + GPS UTC date ddmmyy + GPS UTC time hhmmss + + + + 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 + 0: Action DENIED, 1: Action executed + An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h @@ -34,12 +46,6 @@ The action id - - 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 - 0: Action DENIED, 1: Action executed - - Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. The system setting the mode @@ -117,7 +123,7 @@ - The filtered local position (e.g. fused computer vision and accelerometers). + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) Timestamp (microseconds since unix epoch) X Position Y Position @@ -129,21 +135,20 @@ The global position, as returned by the Global Positioning System (GPS). This is -NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. +NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) Timestamp (microseconds since unix epoch) 0-1: no fix, 2: 2D fix, 3: 3D fix - X Position - Y Position - Z Position in meters - Uncertainty in meters of latitude - Uncertainty in meters of longitude - Overall speed - Heading, in FIXME + Latitude in degrees + Longitude in degrees + Altitude in meters + GPS HDOP + GPS VDOP + GPS ground speed + Compass heading in degrees, 0..360 degrees - The global position, as returned by the Global Positioning System (GPS). This is -NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. Number of satellites visible Global satellite ID 0: Satellite not used, 1: used for localization @@ -153,14 +158,14 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se - The filtered global position (e.g. fused GPS and accelerometers). + The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) Timestamp (microseconds since unix epoch) - X Position - Y Position - Z Position - X Speed - Y Speed - Z Speed + Latitude, in degrees + Longitude, in degrees + Absolute altitude, in meters + X Speed (in Latitude direction, positive: going north) + Y Speed (in Longitude direction, positive: going east) + Z Speed (in Altitude direction, positive: going up) @@ -170,7 +175,7 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se System status flag, see MAV_STATUS ENUM Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 Battery voltage, in millivolts (1 = 1 millivolt) - Motor block status flag, 0: Motors can be switched on (and could be either off or on), 1: Mechanical motor block switch is on, motors cannot be switched on (and are definitely off) + Remaining battery energy: (0%: 0, 100%: 1000) Dropped packets (packets that were corrupted on reception on the MAV) @@ -199,6 +204,18 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 Receive signal strength indicator, 0: 0%, 255: 100% + + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Servo output 1 value, in microseconds + Servo output 2 value, in microseconds + Servo output 3 value, in microseconds + Servo output 4 value, in microseconds + Servo output 5 value, in microseconds + Servo output 6 value, in microseconds + Servo output 7 value, in microseconds + Servo output 8 value, in microseconds + Message encoding a waypoint. This message is emitted to announce @@ -284,20 +301,6 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se local yaw that matches the global yaw orientation - - Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. System ID @@ -315,6 +318,41 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se z position 1 x position 2 + + + Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + Attitude estimation health: 0: poor, 255: excellent + 0: Attitude control disabled, 1: enabled + 0: X, Y position control disabled, 1: enabled + 0: Z position control disabled, 1: enabled + 0: Yaw angle control disabled, 1: enabled + + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + + Read out the safety zone the MAV currently assumes. + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. @@ -334,6 +372,18 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Position yaw: -128: -100%, 127: +100% + + Outputs of the APM navigation controller. The primary use of this message is to check the response and signs +of the controller before actual flight and to assist with tuning controller parameters + Current desired roll in degrees + Current desired pitch in degrees + Current desired heading in degrees + Bearing to current waypoint/target in degrees + Distance to active waypoint in meters + Current altitude error in meters + Current airspeed error in meters/second + + The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. x position @@ -368,20 +418,6 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se 1 to start sending, 0 to stop sending. - - The system which should auto-calibrate -The system component which should auto-calibrate - The current ground-truth rpm - The axis to calibrate: 0 roll, 1 pitch, 2 yaw - The time to average over in ms - - - - The system which should auto-calibrate -The system component which should auto-calibrate - The time to average over in ms - - The system to be controlled roll @@ -394,14 +430,6 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se thrust auto:0, manual:1 - - Name - Timestamp - x - y - z - - Once the MAV sets a new GPS-Local correspondence, this message announces Latitude (WGS84), expressed as * 1E7 @@ -409,33 +437,38 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Altitude(WGS84), expressed as * 1000 - - The calculated airspeed - meters/second - - The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) Latitude / X Position, expressed as * 1E7 Longitude / Y Position, expressed as * 1E7 Altitude / negative Z Position, expressed as * 1000 - Ground X Speed, expressed as m/s * 100 - Ground Y Speed, expressed as m/s * 100 - Ground Z Speed, expressed as m/s * 100 + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 - - - - - + + Metrics typically displayed on a HUD for fixed wing aircraft + Current airspeed in m/s + Current ground speed in m/s + Current heading in degrees + Current altitude (MSL), in meters + Current climb rate in meters/second + Current throttle setting in integer percent, 0 to 100 + - + + Name + Timestamp + x + y + z + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. @@ -452,7 +485,7 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character + Status text message, without null termination character diff --git a/libraries/GCS_MAVLink/message_definitions/pixhawk.xml b/libraries/GCS_MAVLink/message_definitions/pixhawk.xml index f94712375d..6ecb852e26 100644 --- a/libraries/GCS_MAVLink/message_definitions/pixhawk.xml +++ b/libraries/GCS_MAVLink/message_definitions/pixhawk.xml @@ -151,16 +151,6 @@ Number of I2C errors since startup - - Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - 0: Attitude control disabled, 1: enabled -0: X, Y position control disabled, 1: enabled -0: Z position control disabled, 1: enabled -0: Yaw angle control disabled, 1: enabled - - Watchdog ID Number of processes