From 686f96414c87af610a6cf87d13b47ce79716b55b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 15 Dec 2011 18:01:14 +1100 Subject: [PATCH] MAVLink: update to latest mavlink XML this includes the geo-fencing messages --- .../include/ardupilotmega/ardupilotmega.h | 19 +- .../mavlink_msg_fenced_fetch_point.h | 144 ++++++++++++ .../ardupilotmega/mavlink_msg_fenced_point.h | 210 ++++++++++++++++++ .../include/ardupilotmega/testsuite.h | 94 ++++++++ .../include/ardupilotmega/version.h | 2 +- .../ardupilotmega/ardupilotmega.h | 19 +- .../include_v1.0/ardupilotmega/testsuite.h | 94 ++++++++ .../include_v1.0/ardupilotmega/version.h | 2 +- .../message_definitions/ardupilotmega.xml | 24 ++ .../message_definitions/common.xml | 6 +- .../ardupilotmega.xml | 24 ++ .../message_definitions_v1.0/common.xml | 14 -- 12 files changed, 627 insertions(+), 25 deletions(-) create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_fetch_point.h create mode 100644 libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_point.h diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 378cb03948..3944847d76 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 10, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 46, 165, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -106,6 +106,17 @@ enum MAV_CMD }; #endif +/** @brief */ +#ifndef HAVE_ENUM_FENCE_ACTION +#define HAVE_ENUM_FENCE_ACTION +enum FENCE_ACTION +{ + FENCE_ACTION_NONE=0, /* Disable fenced mode | */ + FENCE_ACTION_GUIDED=1, /* Switched to guided mode to return point (fence point 0) | */ + FENCE_ACTION_ENUM_END=2, /* | */ +}; +#endif + // MESSAGE DEFINITIONS #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" @@ -116,6 +127,8 @@ enum MAV_CMD #include "./mavlink_msg_mount_configure.h" #include "./mavlink_msg_mount_control.h" #include "./mavlink_msg_mount_status.h" +#include "./mavlink_msg_fence_point.h" +#include "./mavlink_msg_fence_fetch_point.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_fetch_point.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_fetch_point.h new file mode 100644 index 0000000000..7ae650c71b --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_fetch_point.h @@ -0,0 +1,144 @@ +// MESSAGE FENCED_FETCH_POINT PACKING + +#define MAVLINK_MSG_ID_FENCED_FETCH_POINT 161 + +typedef struct __mavlink_fenced_fetch_point_t +{ + uint8_t idx; ///< point index (first point is 1, 0 is for return point) +} mavlink_fenced_fetch_point_t; + +#define MAVLINK_MSG_ID_FENCED_FETCH_POINT_LEN 1 +#define MAVLINK_MSG_ID_161_LEN 1 + + + +#define MAVLINK_MESSAGE_INFO_FENCED_FETCH_POINT { \ + "FENCED_FETCH_POINT", \ + 1, \ + { { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fenced_fetch_point_t, idx) }, \ + } \ +} + + +/** + * @brief Pack a fenced_fetch_point 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 idx point index (first point is 1, 0 is for return point) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fenced_fetch_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[1]; + _mav_put_uint8_t(buf, 0, idx); + + memcpy(_MAV_PAYLOAD(msg), buf, 1); +#else + mavlink_fenced_fetch_point_t packet; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD(msg), &packet, 1); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCED_FETCH_POINT; + return mavlink_finalize_message(msg, system_id, component_id, 1); +} + +/** + * @brief Pack a fenced_fetch_point message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param idx point index (first point is 1, 0 is for return point) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fenced_fetch_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[1]; + _mav_put_uint8_t(buf, 0, idx); + + memcpy(_MAV_PAYLOAD(msg), buf, 1); +#else + mavlink_fenced_fetch_point_t packet; + packet.idx = idx; + + memcpy(_MAV_PAYLOAD(msg), &packet, 1); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCED_FETCH_POINT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1); +} + +/** + * @brief Encode a fenced_fetch_point 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 fenced_fetch_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fenced_fetch_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fenced_fetch_point_t* fenced_fetch_point) +{ + return mavlink_msg_fenced_fetch_point_pack(system_id, component_id, msg, fenced_fetch_point->idx); +} + +/** + * @brief Send a fenced_fetch_point message + * @param chan MAVLink channel to send the message + * + * @param idx point index (first point is 1, 0 is for return point) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_fenced_fetch_point_send(mavlink_channel_t chan, uint8_t idx) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[1]; + _mav_put_uint8_t(buf, 0, idx); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCED_FETCH_POINT, buf, 1); +#else + mavlink_fenced_fetch_point_t packet; + packet.idx = idx; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCED_FETCH_POINT, (const char *)&packet, 1); +#endif +} + +#endif + +// MESSAGE FENCED_FETCH_POINT UNPACKING + + +/** + * @brief Get field idx from fenced_fetch_point message + * + * @return point index (first point is 1, 0 is for return point) + */ +static inline uint8_t mavlink_msg_fenced_fetch_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Decode a fenced_fetch_point message into a struct + * + * @param msg The message to decode + * @param fenced_fetch_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_fenced_fetch_point_decode(const mavlink_message_t* msg, mavlink_fenced_fetch_point_t* fenced_fetch_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + fenced_fetch_point->idx = mavlink_msg_fenced_fetch_point_get_idx(msg); +#else + memcpy(fenced_fetch_point, _MAV_PAYLOAD(msg), 1); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_point.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_point.h new file mode 100644 index 0000000000..9b849b7763 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_fenced_point.h @@ -0,0 +1,210 @@ +// MESSAGE FENCED_POINT PACKING + +#define MAVLINK_MSG_ID_FENCED_POINT 160 + +typedef struct __mavlink_fenced_point_t +{ + uint8_t idx; ///< point index (first point is 1, 0 is for return point) + uint8_t count; ///< total number of points (for sanity checking) + float lat; ///< Latitude of point + float lng; ///< Longitude of point +} mavlink_fenced_point_t; + +#define MAVLINK_MSG_ID_FENCED_POINT_LEN 10 +#define MAVLINK_MSG_ID_160_LEN 10 + + + +#define MAVLINK_MESSAGE_INFO_FENCED_POINT { \ + "FENCED_POINT", \ + 4, \ + { { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_fenced_point_t, idx) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_fenced_point_t, count) }, \ + { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_fenced_point_t, lat) }, \ + { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_fenced_point_t, lng) }, \ + } \ +} + + +/** + * @brief Pack a fenced_point 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 idx point index (first point is 1, 0 is for return point) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point + * @param lng Longitude of point + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fenced_point_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint8_t(buf, 0, idx); + _mav_put_uint8_t(buf, 1, count); + _mav_put_float(buf, 2, lat); + _mav_put_float(buf, 6, lng); + + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_fenced_point_t packet; + packet.idx = idx; + packet.count = count; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCED_POINT; + return mavlink_finalize_message(msg, system_id, component_id, 10); +} + +/** + * @brief Pack a fenced_point message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param idx point index (first point is 1, 0 is for return point) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point + * @param lng Longitude of point + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_fenced_point_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t idx,uint8_t count,float lat,float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint8_t(buf, 0, idx); + _mav_put_uint8_t(buf, 1, count); + _mav_put_float(buf, 2, lat); + _mav_put_float(buf, 6, lng); + + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_fenced_point_t packet; + packet.idx = idx; + packet.count = count; + packet.lat = lat; + packet.lng = lng; + + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + + msg->msgid = MAVLINK_MSG_ID_FENCED_POINT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10); +} + +/** + * @brief Encode a fenced_point 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 fenced_point C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_fenced_point_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fenced_point_t* fenced_point) +{ + return mavlink_msg_fenced_point_pack(system_id, component_id, msg, fenced_point->idx, fenced_point->count, fenced_point->lat, fenced_point->lng); +} + +/** + * @brief Send a fenced_point message + * @param chan MAVLink channel to send the message + * + * @param idx point index (first point is 1, 0 is for return point) + * @param count total number of points (for sanity checking) + * @param lat Latitude of point + * @param lng Longitude of point + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_fenced_point_send(mavlink_channel_t chan, uint8_t idx, uint8_t count, float lat, float lng) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_uint8_t(buf, 0, idx); + _mav_put_uint8_t(buf, 1, count); + _mav_put_float(buf, 2, lat); + _mav_put_float(buf, 6, lng); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCED_POINT, buf, 10); +#else + mavlink_fenced_point_t packet; + packet.idx = idx; + packet.count = count; + packet.lat = lat; + packet.lng = lng; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCED_POINT, (const char *)&packet, 10); +#endif +} + +#endif + +// MESSAGE FENCED_POINT UNPACKING + + +/** + * @brief Get field idx from fenced_point message + * + * @return point index (first point is 1, 0 is for return point) + */ +static inline uint8_t mavlink_msg_fenced_point_get_idx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field count from fenced_point message + * + * @return total number of points (for sanity checking) + */ +static inline uint8_t mavlink_msg_fenced_point_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field lat from fenced_point message + * + * @return Latitude of point + */ +static inline float mavlink_msg_fenced_point_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 2); +} + +/** + * @brief Get field lng from fenced_point message + * + * @return Longitude of point + */ +static inline float mavlink_msg_fenced_point_get_lng(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 6); +} + +/** + * @brief Decode a fenced_point message into a struct + * + * @param msg The message to decode + * @param fenced_point C-struct to decode the message contents into + */ +static inline void mavlink_msg_fenced_point_decode(const mavlink_message_t* msg, mavlink_fenced_point_t* fenced_point) +{ +#if MAVLINK_NEED_BYTE_SWAP + fenced_point->idx = mavlink_msg_fenced_point_get_idx(msg); + fenced_point->count = mavlink_msg_fenced_point_get_count(msg); + fenced_point->lat = mavlink_msg_fenced_point_get_lat(msg); + fenced_point->lng = mavlink_msg_fenced_point_get_lng(msg); +#else + memcpy(fenced_point, _MAV_PAYLOAD(msg), 10); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 05e04127e2..a86e6fe66f 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -519,6 +519,98 @@ static void mavlink_test_mount_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_fence_point(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_fence_point_t packet_in = { + 5, + 72, + 31.0, + 59.0, + }; + mavlink_fence_point_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.idx = packet_in.idx; + packet1.count = packet_in.count; + packet1.lat = packet_in.lat; + packet1.lng = packet_in.lng; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_fence_point_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_fence_point_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_fence_point_pack(system_id, component_id, &msg , packet1.idx , packet1.count , packet1.lat , packet1.lng ); + mavlink_msg_fence_point_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_fence_point_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.idx , packet1.count , packet1.lat , packet1.lng ); + mavlink_msg_fence_point_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iEmpty + + + + + Disable fenced mode + + + Switched to guided mode to return point (fence point 0) + + @@ -173,5 +183,19 @@ yaw(deg*100) or alt (in cm) depending on mount mode + + + A fence point. Used to set a point when from + GCS -> MAV. Also used to return a point from MAV -> GCS + point index (first point is 1, 0 is for return point) + total number of points (for sanity checking) + Latitude of point + Longitude of point + + + + Request a current fence point from MAV + point index (first point is 1, 0 is for return point) + diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index 5e1cb61bbb..dd4ea84430 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -271,9 +271,9 @@ Waypoint index/ target ID. (see MAV_ROI enum) ROI index (allows a vehicle to manage multiple cameras etc.) Empty - The location of the fixed ROI (see MAV_FRAME) - Empty - Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z NOP - This command is only used to mark the upper limit of the DO commands in the enumeration diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml index 60d86024ac..aed636551d 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml @@ -68,6 +68,16 @@ Empty + + + + + Disable fenced mode + + + Switched to guided mode to return point (fence point 0) + + @@ -173,5 +183,19 @@ yaw(deg*100) or alt (in cm) depending on mount mode + + + A fence point. Used to set a point when from + GCS -> MAV. Also used to return a point from MAV -> GCS + point index (first point is 1, 0 is for return point) + total number of points (for sanity checking) + Latitude of point + Longitude of point + + + + Request a current fence point from MAV + point index (first point is 1, 0 is for return point) + diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml index 1e6738f8b1..8af1eaa413 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/common.xml @@ -604,20 +604,6 @@ Empty Empty - - Sets the region of interest (ROI) for a sensor set or the - vehicle itself. This can then be used by the vehicles control - system to control the vehicle attitude and the attitude of various - devices such as cameras. - - Region of interest mode. (see MAV_ROI enum) - Waypoint index/ target ID. (see MAV_ROI enum) - ROI index (allows a vehicle to manage multiple cameras etc.) - Empty - The location of the fixed ROI (see MAV_FRAME) - Empty - Empty - NOP - This command is only used to mark the upper limit of the DO commands in the enumeration Empty