diff --git a/src/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_fenced_fetch_point.h b/src/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_fenced_fetch_point.h deleted file mode 100644 index 2710ff4a9..000000000 --- a/src/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_fenced_fetch_point.h +++ /dev/null @@ -1,144 +0,0 @@ -// 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, 33); -} - -/** - * @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, 33); -} - -/** - * @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, 33); -#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, 33); -#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/src/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_fenced_point.h b/src/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_fenced_point.h deleted file mode 100644 index 46cf41fe0..000000000 --- a/src/GCS_MAVLink/include_v1.0/ardupilotmega/mavlink_msg_fenced_point.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE FENCED_POINT PACKING - -#define MAVLINK_MSG_ID_FENCED_POINT 160 - -typedef struct __mavlink_fenced_point_t -{ - float lat; ///< Latitude of point - float lng; ///< Longitude of point - uint8_t idx; ///< point index (first point is 1, 0 is for return point) - uint8_t count; ///< total number of points (for sanity checking) -} 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, \ - { { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_fenced_point_t, lat) }, \ - { "lng", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_fenced_point_t, lng) }, \ - { "idx", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fenced_point_t, idx) }, \ - { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_fenced_point_t, count) }, \ - } \ -} - - -/** - * @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_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, idx); - _mav_put_uint8_t(buf, 9, count); - - memcpy(_MAV_PAYLOAD(msg), buf, 10); -#else - mavlink_fenced_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.idx = idx; - packet.count = count; - - memcpy(_MAV_PAYLOAD(msg), &packet, 10); -#endif - - msg->msgid = MAVLINK_MSG_ID_FENCED_POINT; - return mavlink_finalize_message(msg, system_id, component_id, 10, 225); -} - -/** - * @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_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, idx); - _mav_put_uint8_t(buf, 9, count); - - memcpy(_MAV_PAYLOAD(msg), buf, 10); -#else - mavlink_fenced_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.idx = idx; - packet.count = count; - - 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, 225); -} - -/** - * @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_float(buf, 0, lat); - _mav_put_float(buf, 4, lng); - _mav_put_uint8_t(buf, 8, idx); - _mav_put_uint8_t(buf, 9, count); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCED_POINT, buf, 10, 225); -#else - mavlink_fenced_point_t packet; - packet.lat = lat; - packet.lng = lng; - packet.idx = idx; - packet.count = count; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCED_POINT, (const char *)&packet, 10, 225); -#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, 8); -} - -/** - * @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, 9); -} - -/** - * @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, 0); -} - -/** - * @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, 4); -} - -/** - * @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->lat = mavlink_msg_fenced_point_get_lat(msg); - fenced_point->lng = mavlink_msg_fenced_point_get_lng(msg); - fenced_point->idx = mavlink_msg_fenced_point_get_idx(msg); - fenced_point->count = mavlink_msg_fenced_point_get_count(msg); -#else - memcpy(fenced_point, _MAV_PAYLOAD(msg), 10); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_action.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_action.h deleted file mode 100644 index b34fbb342..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_action.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE ACTION PACKING - -#define MAVLINK_MSG_ID_ACTION 10 - -typedef struct __mavlink_action_t -{ - uint8_t target; ///< The system executing the action - uint8_t target_component; ///< The component executing the action - uint8_t action; ///< The action id -} mavlink_action_t; - -#define MAVLINK_MSG_ID_ACTION_LEN 3 -#define MAVLINK_MSG_ID_10_LEN 3 - - - -#define MAVLINK_MESSAGE_INFO_ACTION { \ - "ACTION", \ - 3, \ - { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_action_t, target) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_action_t, target_component) }, \ - { "action", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_action_t, action) }, \ - } \ -} - - -/** - * @brief Pack a action 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 The system executing the action - * @param target_component The component executing the action - * @param action The action id - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint8_t target_component, uint8_t action) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, action); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_action_t packet; - packet.target = target; - packet.target_component = target_component; - packet.action = action; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_ACTION; - return mavlink_finalize_message(msg, system_id, component_id, 3, 60); -} - -/** - * @brief Pack a action 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 target The system executing the action - * @param target_component The component executing the action - * @param action The action id - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t target_component,uint8_t action) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, action); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_action_t packet; - packet.target = target; - packet.target_component = target_component; - packet.action = action; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_ACTION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 60); -} - -/** - * @brief Encode a action 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 action C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_t* action) -{ - return mavlink_msg_action_pack(system_id, component_id, msg, action->target, action->target_component, action->action); -} - -/** - * @brief Send a action message - * @param chan MAVLink channel to send the message - * - * @param target The system executing the action - * @param target_component The component executing the action - * @param action The action id - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, action); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTION, buf, 3, 60); -#else - mavlink_action_t packet; - packet.target = target; - packet.target_component = target_component; - packet.action = action; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTION, (const char *)&packet, 3, 60); -#endif -} - -#endif - -// MESSAGE ACTION UNPACKING - - -/** - * @brief Get field target from action message - * - * @return The system executing the action - */ -static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from action message - * - * @return The component executing the action - */ -static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field action from action message - * - * @return The action id - */ -static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a action message into a struct - * - * @param msg The message to decode - * @param action C-struct to decode the message contents into - */ -static inline void mavlink_msg_action_decode(const mavlink_message_t* msg, mavlink_action_t* action) -{ -#if MAVLINK_NEED_BYTE_SWAP - action->target = mavlink_msg_action_get_target(msg); - action->target_component = mavlink_msg_action_get_target_component(msg); - action->action = mavlink_msg_action_get_action(msg); -#else - memcpy(action, _MAV_PAYLOAD(msg), 3); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_action_ack.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_action_ack.h deleted file mode 100644 index f5da3cb06..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_action_ack.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE ACTION_ACK PACKING - -#define MAVLINK_MSG_ID_ACTION_ACK 9 - -typedef struct __mavlink_action_ack_t -{ - uint8_t action; ///< The action id - uint8_t result; ///< 0: Action DENIED, 1: Action executed -} mavlink_action_ack_t; - -#define MAVLINK_MSG_ID_ACTION_ACK_LEN 2 -#define MAVLINK_MSG_ID_9_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_ACTION_ACK { \ - "ACTION_ACK", \ - 2, \ - { { "action", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_action_ack_t, action) }, \ - { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_action_ack_t, result) }, \ - } \ -} - - -/** - * @brief Pack a action_ack 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 action The action id - * @param result 0: Action DENIED, 1: Action executed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t action, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, action); - _mav_put_uint8_t(buf, 1, result); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_action_ack_t packet; - packet.action = action; - packet.result = result; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - return mavlink_finalize_message(msg, system_id, component_id, 2, 219); -} - -/** - * @brief Pack a action_ack 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 action The action id - * @param result 0: Action DENIED, 1: Action executed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_action_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t action,uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, action); - _mav_put_uint8_t(buf, 1, result); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_action_ack_t packet; - packet.action = action; - packet.result = result; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 219); -} - -/** - * @brief Encode a action_ack 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 action_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_action_ack_t* action_ack) -{ - return mavlink_msg_action_ack_pack(system_id, component_id, msg, action_ack->action, action_ack->result); -} - -/** - * @brief Send a action_ack message - * @param chan MAVLink channel to send the message - * - * @param action The action id - * @param result 0: Action DENIED, 1: Action executed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, action); - _mav_put_uint8_t(buf, 1, result); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTION_ACK, buf, 2, 219); -#else - mavlink_action_ack_t packet; - packet.action = action; - packet.result = result; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTION_ACK, (const char *)&packet, 2, 219); -#endif -} - -#endif - -// MESSAGE ACTION_ACK UNPACKING - - -/** - * @brief Get field action from action_ack message - * - * @return The action id - */ -static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field result from action_ack message - * - * @return 0: Action DENIED, 1: Action executed - */ -static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a action_ack message into a struct - * - * @param msg The message to decode - * @param action_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_action_ack_decode(const mavlink_message_t* msg, mavlink_action_ack_t* action_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - action_ack->action = mavlink_msg_action_ack_get_action(msg); - action_ack->result = mavlink_msg_action_ack_get_result(msg); -#else - memcpy(action_ack, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_boot.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_boot.h deleted file mode 100644 index 679a0a346..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_boot.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE BOOT PACKING - -#define MAVLINK_MSG_ID_BOOT 1 - -typedef struct __mavlink_boot_t -{ - uint32_t version; ///< The onboard software version -} mavlink_boot_t; - -#define MAVLINK_MSG_ID_BOOT_LEN 4 -#define MAVLINK_MSG_ID_1_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_BOOT { \ - "BOOT", \ - 1, \ - { { "version", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_boot_t, version) }, \ - } \ -} - - -/** - * @brief Pack a boot 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 version The onboard software version - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t version) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint32_t(buf, 0, version); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_boot_t packet; - packet.version = version; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_BOOT; - return mavlink_finalize_message(msg, system_id, component_id, 4, 39); -} - -/** - * @brief Pack a boot 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 version The onboard software version - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_boot_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t version) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint32_t(buf, 0, version); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_boot_t packet; - packet.version = version; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_BOOT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 39); -} - -/** - * @brief Encode a boot 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 boot C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_boot_t* boot) -{ - return mavlink_msg_boot_pack(system_id, component_id, msg, boot->version); -} - -/** - * @brief Send a boot message - * @param chan MAVLink channel to send the message - * - * @param version The onboard software version - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint32_t(buf, 0, version); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BOOT, buf, 4, 39); -#else - mavlink_boot_t packet; - packet.version = version; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BOOT, (const char *)&packet, 4, 39); -#endif -} - -#endif - -// MESSAGE BOOT UNPACKING - - -/** - * @brief Get field version from boot message - * - * @return The onboard software version - */ -static inline uint32_t mavlink_msg_boot_get_version(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Decode a boot message into a struct - * - * @param msg The message to decode - * @param boot C-struct to decode the message contents into - */ -static inline void mavlink_msg_boot_decode(const mavlink_message_t* msg, mavlink_boot_t* boot) -{ -#if MAVLINK_NEED_BYTE_SWAP - boot->version = mavlink_msg_boot_get_version(msg); -#else - memcpy(boot, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_command.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_command.h deleted file mode 100644 index 5e477801f..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_command.h +++ /dev/null @@ -1,298 +0,0 @@ -// MESSAGE COMMAND PACKING - -#define MAVLINK_MSG_ID_COMMAND 75 - -typedef struct __mavlink_command_t -{ - float param1; ///< Parameter 1, as defined by MAV_CMD enum. - float param2; ///< Parameter 2, as defined by MAV_CMD enum. - float param3; ///< Parameter 3, as defined by MAV_CMD enum. - float param4; ///< Parameter 4, as defined by MAV_CMD enum. - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) -} mavlink_command_t; - -#define MAVLINK_MSG_ID_COMMAND_LEN 20 -#define MAVLINK_MSG_ID_75_LEN 20 - - - -#define MAVLINK_MESSAGE_INFO_COMMAND { \ - "COMMAND", \ - 8, \ - { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_t, param1) }, \ - { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_t, param2) }, \ - { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_t, param3) }, \ - { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_t, param4) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_command_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_command_t, target_component) }, \ - { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_command_t, command) }, \ - { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_command_t, confirmation) }, \ - } \ -} - - -/** - * @brief Pack a command 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 which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, command); - _mav_put_uint8_t(buf, 19, confirmation); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_command_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command = command; - packet.confirmation = confirmation; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND; - return mavlink_finalize_message(msg, system_id, component_id, 20, 133); -} - -/** - * @brief Pack a command 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 target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_command_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 command,uint8_t confirmation,float param1,float param2,float param3,float param4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, command); - _mav_put_uint8_t(buf, 19, confirmation); - - memcpy(_MAV_PAYLOAD(msg), buf, 20); -#else - mavlink_command_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command = command; - packet.confirmation = confirmation; - - memcpy(_MAV_PAYLOAD(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_COMMAND; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 133); -} - -/** - * @brief Encode a command 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 command C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_t* command) -{ - return mavlink_msg_command_pack(system_id, component_id, msg, command->target_system, command->target_component, command->command, command->confirmation, command->param1, command->param2, command->param3, command->param4); -} - -/** - * @brief Send a command message - * @param chan MAVLink channel to send the message - * - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - _mav_put_uint8_t(buf, 18, command); - _mav_put_uint8_t(buf, 19, confirmation); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND, buf, 20, 133); -#else - mavlink_command_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.target_system = target_system; - packet.target_component = target_component; - packet.command = command; - packet.confirmation = confirmation; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND, (const char *)&packet, 20, 133); -#endif -} - -#endif - -// MESSAGE COMMAND UNPACKING - - -/** - * @brief Get field target_system from command message - * - * @return System which should execute the command - */ -static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from command message - * - * @return Component which should execute the command, 0 for all components - */ -static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field command from command message - * - * @return Command ID, as defined by MAV_CMD enum. - */ -static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 18); -} - -/** - * @brief Get field confirmation from command message - * - * @return 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - */ -static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 19); -} - -/** - * @brief Get field param1 from command message - * - * @return Parameter 1, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param2 from command message - * - * @return Parameter 2, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field param3 from command message - * - * @return Parameter 3, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field param4 from command message - * - * @return Parameter 4, as defined by MAV_CMD enum. - */ -static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a command message into a struct - * - * @param msg The message to decode - * @param command C-struct to decode the message contents into - */ -static inline void mavlink_msg_command_decode(const mavlink_message_t* msg, mavlink_command_t* command) -{ -#if MAVLINK_NEED_BYTE_SWAP - command->param1 = mavlink_msg_command_get_param1(msg); - command->param2 = mavlink_msg_command_get_param2(msg); - command->param3 = mavlink_msg_command_get_param3(msg); - command->param4 = mavlink_msg_command_get_param4(msg); - command->target_system = mavlink_msg_command_get_target_system(msg); - command->target_component = mavlink_msg_command_get_target_component(msg); - command->command = mavlink_msg_command_get_command(msg); - command->confirmation = mavlink_msg_command_get_confirmation(msg); -#else - memcpy(command, _MAV_PAYLOAD(msg), 20); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_control_status.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_control_status.h deleted file mode 100644 index 1723a6b80..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_control_status.h +++ /dev/null @@ -1,298 +0,0 @@ -// 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; - -#define MAVLINK_MSG_ID_CONTROL_STATUS_LEN 8 -#define MAVLINK_MSG_ID_52_LEN 8 - - - -#define MAVLINK_MESSAGE_INFO_CONTROL_STATUS { \ - "CONTROL_STATUS", \ - 8, \ - { { "position_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_control_status_t, position_fix) }, \ - { "vision_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_control_status_t, vision_fix) }, \ - { "gps_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_control_status_t, gps_fix) }, \ - { "ahrs_health", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_control_status_t, ahrs_health) }, \ - { "control_att", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_control_status_t, control_att) }, \ - { "control_pos_xy", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_control_status_t, control_pos_xy) }, \ - { "control_pos_z", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_control_status_t, control_pos_z) }, \ - { "control_pos_yaw", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_control_status_t, control_pos_yaw) }, \ - } \ -} - - -/** - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint8_t(buf, 0, position_fix); - _mav_put_uint8_t(buf, 1, vision_fix); - _mav_put_uint8_t(buf, 2, gps_fix); - _mav_put_uint8_t(buf, 3, ahrs_health); - _mav_put_uint8_t(buf, 4, control_att); - _mav_put_uint8_t(buf, 5, control_pos_xy); - _mav_put_uint8_t(buf, 6, control_pos_z); - _mav_put_uint8_t(buf, 7, control_pos_yaw); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_control_status_t packet; - packet.position_fix = position_fix; - packet.vision_fix = vision_fix; - packet.gps_fix = gps_fix; - packet.ahrs_health = ahrs_health; - packet.control_att = control_att; - packet.control_pos_xy = control_pos_xy; - packet.control_pos_z = control_pos_z; - packet.control_pos_yaw = control_pos_yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - return mavlink_finalize_message(msg, system_id, component_id, 8, 157); -} - -/** - * @brief Pack a control_status 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 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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint8_t(buf, 0, position_fix); - _mav_put_uint8_t(buf, 1, vision_fix); - _mav_put_uint8_t(buf, 2, gps_fix); - _mav_put_uint8_t(buf, 3, ahrs_health); - _mav_put_uint8_t(buf, 4, control_att); - _mav_put_uint8_t(buf, 5, control_pos_xy); - _mav_put_uint8_t(buf, 6, control_pos_z); - _mav_put_uint8_t(buf, 7, control_pos_yaw); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_control_status_t packet; - packet.position_fix = position_fix; - packet.vision_fix = vision_fix; - packet.gps_fix = gps_fix; - packet.ahrs_health = ahrs_health; - packet.control_att = control_att; - packet.control_pos_xy = control_pos_xy; - packet.control_pos_z = control_pos_z; - packet.control_pos_yaw = control_pos_yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 157); -} - -/** - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint8_t(buf, 0, position_fix); - _mav_put_uint8_t(buf, 1, vision_fix); - _mav_put_uint8_t(buf, 2, gps_fix); - _mav_put_uint8_t(buf, 3, ahrs_health); - _mav_put_uint8_t(buf, 4, control_att); - _mav_put_uint8_t(buf, 5, control_pos_xy); - _mav_put_uint8_t(buf, 6, control_pos_z); - _mav_put_uint8_t(buf, 7, control_pos_yaw); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONTROL_STATUS, buf, 8, 157); -#else - mavlink_control_status_t packet; - packet.position_fix = position_fix; - packet.vision_fix = vision_fix; - packet.gps_fix = gps_fix; - packet.ahrs_health = ahrs_health; - packet.control_att = control_att; - packet.control_pos_xy = control_pos_xy; - packet.control_pos_z = control_pos_z; - packet.control_pos_yaw = control_pos_yaw; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CONTROL_STATUS, (const char *)&packet, 8, 157); -#endif -} - -#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 _MAV_RETURN_uint8_t(msg, 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 _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @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 _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @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 _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @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 _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @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 _MAV_RETURN_uint8_t(msg, 5); -} - -/** - * @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 _MAV_RETURN_uint8_t(msg, 6); -} - -/** - * @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 _MAV_RETURN_uint8_t(msg, 7); -} - -/** - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP - 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); -#else - memcpy(control_status, _MAV_PAYLOAD(msg), 8); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_extended_message.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_extended_message.h deleted file mode 100644 index 2c42739be..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_extended_message.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE EXTENDED_MESSAGE PACKING - -#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 - -typedef struct __mavlink_extended_message_t -{ - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t protocol_flags; ///< Retransmission / ACK flags -} mavlink_extended_message_t; - -#define MAVLINK_MSG_ID_EXTENDED_MESSAGE_LEN 3 -#define MAVLINK_MSG_ID_255_LEN 3 - - - -#define MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE { \ - "EXTENDED_MESSAGE", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_extended_message_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_extended_message_t, target_component) }, \ - { "protocol_flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_extended_message_t, protocol_flags) }, \ - } \ -} - - -/** - * @brief Pack a extended_message 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 which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param protocol_flags Retransmission / ACK flags - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_extended_message_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t protocol_flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, protocol_flags); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_extended_message_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.protocol_flags = protocol_flags; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; - return mavlink_finalize_message(msg, system_id, component_id, 3, 247); -} - -/** - * @brief Pack a extended_message 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 target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param protocol_flags Retransmission / ACK flags - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_extended_message_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 protocol_flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, protocol_flags); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_extended_message_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.protocol_flags = protocol_flags; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 247); -} - -/** - * @brief Encode a extended_message 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 extended_message C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_extended_message_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_extended_message_t* extended_message) -{ - return mavlink_msg_extended_message_pack(system_id, component_id, msg, extended_message->target_system, extended_message->target_component, extended_message->protocol_flags); -} - -/** - * @brief Send a extended_message message - * @param chan MAVLink channel to send the message - * - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param protocol_flags Retransmission / ACK flags - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_extended_message_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t protocol_flags) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, protocol_flags); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTENDED_MESSAGE, buf, 3, 247); -#else - mavlink_extended_message_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.protocol_flags = protocol_flags; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTENDED_MESSAGE, (const char *)&packet, 3, 247); -#endif -} - -#endif - -// MESSAGE EXTENDED_MESSAGE UNPACKING - - -/** - * @brief Get field target_system from extended_message message - * - * @return System which should execute the command - */ -static inline uint8_t mavlink_msg_extended_message_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from extended_message message - * - * @return Component which should execute the command, 0 for all components - */ -static inline uint8_t mavlink_msg_extended_message_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field protocol_flags from extended_message message - * - * @return Retransmission / ACK flags - */ -static inline uint8_t mavlink_msg_extended_message_get_protocol_flags(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a extended_message message into a struct - * - * @param msg The message to decode - * @param extended_message C-struct to decode the message contents into - */ -static inline void mavlink_msg_extended_message_decode(const mavlink_message_t* msg, mavlink_extended_message_t* extended_message) -{ -#if MAVLINK_NEED_BYTE_SWAP - extended_message->target_system = mavlink_msg_extended_message_get_target_system(msg); - extended_message->target_component = mavlink_msg_extended_message_get_target_component(msg); - extended_message->protocol_flags = mavlink_msg_extended_message_get_protocol_flags(msg); -#else - memcpy(extended_message, _MAV_PAYLOAD(msg), 3); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h deleted file mode 100644 index 6e902393d..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_global_position.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE GLOBAL_POSITION PACKING - -#define MAVLINK_MSG_ID_GLOBAL_POSITION 33 - -typedef struct __mavlink_global_position_t -{ - uint64_t usec; ///< Timestamp (microseconds since unix epoch) - 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; - -#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 -#define MAVLINK_MSG_ID_33_LEN 32 - - - -#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION { \ - "GLOBAL_POSITION", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_t, usec) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_position_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_position_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_t, alt) }, \ - { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_t, vz) }, \ - } \ -} - - -/** - * @brief Pack a global_position message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds since unix epoch) - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_global_position_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - return mavlink_finalize_message(msg, system_id, component_id, 32, 147); -} - -/** - * @brief Pack a global_position 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 usec Timestamp (microseconds since unix epoch) - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_global_position_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 147); -} - -/** - * @brief Encode a global_position 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 global_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_t* global_position) -{ - return mavlink_msg_global_position_pack(system_id, component_id, msg, global_position->usec, global_position->lat, global_position->lon, global_position->alt, global_position->vx, global_position->vy, global_position->vz); -} - -/** - * @brief Send a global_position message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds since unix epoch) - * @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 - -static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION, buf, 32, 147); -#else - mavlink_global_position_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION, (const char *)&packet, 32, 147); -#endif -} - -#endif - -// MESSAGE GLOBAL_POSITION UNPACKING - - -/** - * @brief Get field usec from global_position message - * - * @return Timestamp (microseconds since unix epoch) - */ -static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field lat from global_position message - * - * @return Latitude, in degrees - */ -static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field lon from global_position message - * - * @return Longitude, in degrees - */ -static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field alt from global_position message - * - * @return Absolute altitude, in meters - */ -static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field vx from global_position message - * - * @return X Speed (in Latitude direction, positive: going north) - */ -static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field vy from global_position message - * - * @return Y Speed (in Longitude direction, positive: going east) - */ -static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field vz from global_position message - * - * @return Z Speed (in Altitude direction, positive: going up) - */ -static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a global_position message into a struct - * - * @param msg The message to decode - * @param global_position C-struct to decode the message contents into - */ -static inline void mavlink_msg_global_position_decode(const mavlink_message_t* msg, mavlink_global_position_t* global_position) -{ -#if MAVLINK_NEED_BYTE_SWAP - global_position->usec = mavlink_msg_global_position_get_usec(msg); - global_position->lat = mavlink_msg_global_position_get_lat(msg); - global_position->lon = mavlink_msg_global_position_get_lon(msg); - global_position->alt = mavlink_msg_global_position_get_alt(msg); - global_position->vx = mavlink_msg_global_position_get_vx(msg); - global_position->vy = mavlink_msg_global_position_get_vy(msg); - global_position->vz = mavlink_msg_global_position_get_vz(msg); -#else - memcpy(global_position, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_local_origin_set.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_local_origin_set.h deleted file mode 100644 index 344c76aaa..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_local_origin_set.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE GPS_LOCAL_ORIGIN_SET PACKING - -#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET 49 - -typedef struct __mavlink_gps_local_origin_set_t -{ - int32_t latitude; ///< Latitude (WGS84), expressed as * 1E7 - int32_t longitude; ///< Longitude (WGS84), expressed as * 1E7 - int32_t altitude; ///< Altitude(WGS84), expressed as * 1000 -} mavlink_gps_local_origin_set_t; - -#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN 12 -#define MAVLINK_MSG_ID_49_LEN 12 - - - -#define MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET { \ - "GPS_LOCAL_ORIGIN_SET", \ - 3, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_local_origin_set_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_local_origin_set_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_local_origin_set_t, altitude) }, \ - } \ -} - - -/** - * @brief Pack a gps_local_origin_set 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 latitude Latitude (WGS84), expressed as * 1E7 - * @param longitude Longitude (WGS84), expressed as * 1E7 - * @param altitude Altitude(WGS84), expressed as * 1000 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_gps_local_origin_set_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - return mavlink_finalize_message(msg, system_id, component_id, 12, 14); -} - -/** - * @brief Pack a gps_local_origin_set 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 latitude Latitude (WGS84), expressed as * 1E7 - * @param longitude Longitude (WGS84), expressed as * 1E7 - * @param altitude Altitude(WGS84), expressed as * 1000 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_local_origin_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - - memcpy(_MAV_PAYLOAD(msg), buf, 12); -#else - mavlink_gps_local_origin_set_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - - memcpy(_MAV_PAYLOAD(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 14); -} - -/** - * @brief Encode a gps_local_origin_set 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 gps_local_origin_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_local_origin_set_t* gps_local_origin_set) -{ - return mavlink_msg_gps_local_origin_set_pack(system_id, component_id, msg, gps_local_origin_set->latitude, gps_local_origin_set->longitude, gps_local_origin_set->altitude); -} - -/** - * @brief Send a gps_local_origin_set message - * @param chan MAVLink channel to send the message - * - * @param latitude Latitude (WGS84), expressed as * 1E7 - * @param longitude Longitude (WGS84), expressed as * 1E7 - * @param altitude Altitude(WGS84), expressed as * 1000 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET, buf, 12, 14); -#else - mavlink_gps_local_origin_set_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET, (const char *)&packet, 12, 14); -#endif -} - -#endif - -// MESSAGE GPS_LOCAL_ORIGIN_SET UNPACKING - - -/** - * @brief Get field latitude from gps_local_origin_set message - * - * @return Latitude (WGS84), expressed as * 1E7 - */ -static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from gps_local_origin_set message - * - * @return Longitude (WGS84), expressed as * 1E7 - */ -static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from gps_local_origin_set message - * - * @return Altitude(WGS84), expressed as * 1000 - */ -static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a gps_local_origin_set message into a struct - * - * @param msg The message to decode - * @param gps_local_origin_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_local_origin_set_decode(const mavlink_message_t* msg, mavlink_gps_local_origin_set_t* gps_local_origin_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_local_origin_set->latitude = mavlink_msg_gps_local_origin_set_get_latitude(msg); - gps_local_origin_set->longitude = mavlink_msg_gps_local_origin_set_get_longitude(msg); - gps_local_origin_set->altitude = mavlink_msg_gps_local_origin_set_get_altitude(msg); -#else - memcpy(gps_local_origin_set, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h deleted file mode 100644 index 17f629452..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_raw.h +++ /dev/null @@ -1,320 +0,0 @@ -// MESSAGE GPS_RAW PACKING - -#define MAVLINK_MSG_ID_GPS_RAW 32 - -typedef struct __mavlink_gps_raw_t -{ - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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 - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. -} mavlink_gps_raw_t; - -#define MAVLINK_MSG_ID_GPS_RAW_LEN 37 -#define MAVLINK_MSG_ID_32_LEN 37 - - - -#define MAVLINK_MESSAGE_INFO_GPS_RAW { \ - "GPS_RAW", \ - 9, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_t, usec) }, \ - { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gps_raw_t, lat) }, \ - { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gps_raw_t, lon) }, \ - { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gps_raw_t, alt) }, \ - { "eph", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gps_raw_t, eph) }, \ - { "epv", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gps_raw_t, epv) }, \ - { "v", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gps_raw_t, v) }, \ - { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gps_raw_t, hdg) }, \ - { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_gps_raw_t, fix_type) }, \ - } \ -} - - -/** - * @brief Pack a gps_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 usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude 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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[37]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, eph); - _mav_put_float(buf, 24, epv); - _mav_put_float(buf, 28, v); - _mav_put_float(buf, 32, hdg); - _mav_put_uint8_t(buf, 36, fix_type); - - memcpy(_MAV_PAYLOAD(msg), buf, 37); -#else - mavlink_gps_raw_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.v = v; - packet.hdg = hdg; - packet.fix_type = fix_type; - - memcpy(_MAV_PAYLOAD(msg), &packet, 37); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - return mavlink_finalize_message(msg, system_id, component_id, 37, 220); -} - -/** - * @brief Pack a gps_raw message on a channel - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude 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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[37]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, eph); - _mav_put_float(buf, 24, epv); - _mav_put_float(buf, 28, v); - _mav_put_float(buf, 32, hdg); - _mav_put_uint8_t(buf, 36, fix_type); - - memcpy(_MAV_PAYLOAD(msg), buf, 37); -#else - mavlink_gps_raw_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.v = v; - packet.hdg = hdg; - packet.fix_type = fix_type; - - memcpy(_MAV_PAYLOAD(msg), &packet, 37); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37, 220); -} - -/** - * @brief Encode a gps_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 gps_raw C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_t* gps_raw) -{ - return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg); -} - -/** - * @brief Send a gps_raw message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude 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 - -static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[37]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, lat); - _mav_put_float(buf, 12, lon); - _mav_put_float(buf, 16, alt); - _mav_put_float(buf, 20, eph); - _mav_put_float(buf, 24, epv); - _mav_put_float(buf, 28, v); - _mav_put_float(buf, 32, hdg); - _mav_put_uint8_t(buf, 36, fix_type); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, buf, 37, 220); -#else - mavlink_gps_raw_t packet; - packet.usec = usec; - packet.lat = lat; - packet.lon = lon; - packet.alt = alt; - packet.eph = eph; - packet.epv = epv; - packet.v = v; - packet.hdg = hdg; - packet.fix_type = fix_type; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, (const char *)&packet, 37, 220); -#endif -} - -#endif - -// MESSAGE GPS_RAW UNPACKING - - -/** - * @brief Get field usec from gps_raw message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field fix_type from gps_raw message - * - * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - */ -static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 36); -} - -/** - * @brief Get field lat from gps_raw message - * - * @return Latitude in degrees - */ -static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field lon from gps_raw message - * - * @return Longitude in degrees - */ -static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field alt from gps_raw message - * - * @return Altitude in meters - */ -static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field eph from gps_raw message - * - * @return GPS HDOP - */ -static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field epv from gps_raw message - * - * @return GPS VDOP - */ -static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field v from gps_raw message - * - * @return GPS ground speed - */ -static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Get field hdg from gps_raw message - * - * @return Compass heading in degrees, 0..360 degrees - */ -static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 32); -} - -/** - * @brief Decode a gps_raw message into a struct - * - * @param msg The message to decode - * @param gps_raw C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavlink_gps_raw_t* gps_raw) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_raw->usec = mavlink_msg_gps_raw_get_usec(msg); - gps_raw->lat = mavlink_msg_gps_raw_get_lat(msg); - gps_raw->lon = mavlink_msg_gps_raw_get_lon(msg); - gps_raw->alt = mavlink_msg_gps_raw_get_alt(msg); - gps_raw->eph = mavlink_msg_gps_raw_get_eph(msg); - gps_raw->epv = mavlink_msg_gps_raw_get_epv(msg); - gps_raw->v = mavlink_msg_gps_raw_get_v(msg); - gps_raw->hdg = mavlink_msg_gps_raw_get_hdg(msg); - gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); -#else - memcpy(gps_raw, _MAV_PAYLOAD(msg), 37); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h deleted file mode 100644 index 97ce14907..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_gps_set_global_origin.h +++ /dev/null @@ -1,232 +0,0 @@ -// MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING - -#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 - -typedef struct __mavlink_gps_set_global_origin_t -{ - int32_t latitude; ///< global position * 1E7 - int32_t longitude; ///< global position * 1E7 - int32_t altitude; ///< global position * 1000 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_gps_set_global_origin_t; - -#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 -#define MAVLINK_MSG_ID_48_LEN 14 - - - -#define MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN { \ - "GPS_SET_GLOBAL_ORIGIN", \ - 5, \ - { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_set_global_origin_t, latitude) }, \ - { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_set_global_origin_t, longitude) }, \ - { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_set_global_origin_t, altitude) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gps_set_global_origin_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_gps_set_global_origin_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a gps_set_global_origin 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 latitude global position * 1E7 - * @param longitude global position * 1E7 - * @param altitude global position * 1000 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[14]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 14); -#else - mavlink_gps_set_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 14); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - return mavlink_finalize_message(msg, system_id, component_id, 14, 170); -} - -/** - * @brief Pack a gps_set_global_origin 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 target_system System ID - * @param target_component Component ID - * @param latitude global position * 1E7 - * @param longitude global position * 1E7 - * @param altitude global position * 1000 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int32_t latitude,int32_t longitude,int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[14]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 14); -#else - mavlink_gps_set_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 14); -#endif - - msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 170); -} - -/** - * @brief Encode a gps_set_global_origin 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 gps_set_global_origin C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_set_global_origin_t* gps_set_global_origin) -{ - return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->latitude, gps_set_global_origin->longitude, gps_set_global_origin->altitude); -} - -/** - * @brief Send a gps_set_global_origin message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param latitude global position * 1E7 - * @param longitude global position * 1E7 - * @param altitude global position * 1000 - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[14]; - _mav_put_int32_t(buf, 0, latitude); - _mav_put_int32_t(buf, 4, longitude); - _mav_put_int32_t(buf, 8, altitude); - _mav_put_uint8_t(buf, 12, target_system); - _mav_put_uint8_t(buf, 13, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, buf, 14, 170); -#else - mavlink_gps_set_global_origin_t packet; - packet.latitude = latitude; - packet.longitude = longitude; - packet.altitude = altitude; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, (const char *)&packet, 14, 170); -#endif -} - -#endif - -// MESSAGE GPS_SET_GLOBAL_ORIGIN UNPACKING - - -/** - * @brief Get field target_system from gps_set_global_origin message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 12); -} - -/** - * @brief Get field target_component from gps_set_global_origin message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 13); -} - -/** - * @brief Get field latitude from gps_set_global_origin message - * - * @return global position * 1E7 - */ -static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field longitude from gps_set_global_origin message - * - * @return global position * 1E7 - */ -static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field altitude from gps_set_global_origin message - * - * @return global position * 1000 - */ -static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a gps_set_global_origin message into a struct - * - * @param msg The message to decode - * @param gps_set_global_origin C-struct to decode the message contents into - */ -static inline void mavlink_msg_gps_set_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_set_global_origin_t* gps_set_global_origin) -{ -#if MAVLINK_NEED_BYTE_SWAP - gps_set_global_origin->latitude = mavlink_msg_gps_set_global_origin_get_latitude(msg); - gps_set_global_origin->longitude = mavlink_msg_gps_set_global_origin_get_longitude(msg); - gps_set_global_origin->altitude = mavlink_msg_gps_set_global_origin_get_altitude(msg); - gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg); - gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg); -#else - memcpy(gps_set_global_origin, _MAV_PAYLOAD(msg), 14); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_gps.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_gps.h new file mode 100644 index 000000000..75ab6835d --- /dev/null +++ b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_gps.h @@ -0,0 +1,427 @@ +// MESSAGE HIL_GPS PACKING + +#define MAVLINK_MSG_ID_HIL_GPS 113 + +typedef struct __mavlink_hil_gps_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude (WGS84), in degrees * 1E7 + int32_t lon; ///< Longitude (WGS84), in degrees * 1E7 + int32_t alt; ///< Altitude (WGS84), in meters * 1000 (positive for up) + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 + int16_t vn; ///< GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + int16_t ve; ///< GPS velocity in cm/s in EAST direction in earth-fixed NED frame + int16_t vd; ///< GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + uint16_t cog; ///< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 +} mavlink_hil_gps_t; + +#define MAVLINK_MSG_ID_HIL_GPS_LEN 36 +#define MAVLINK_MSG_ID_113_LEN 36 + +#define MAVLINK_MSG_ID_HIL_GPS_CRC 124 +#define MAVLINK_MSG_ID_113_CRC 124 + + + +#define MAVLINK_MESSAGE_INFO_HIL_GPS { \ + "HIL_GPS", \ + 13, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_hil_gps_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_hil_gps_t, alt) }, \ + { "eph", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_hil_gps_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_hil_gps_t, epv) }, \ + { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_hil_gps_t, vel) }, \ + { "vn", NULL, MAVLINK_TYPE_INT16_T, 0, 26, offsetof(mavlink_hil_gps_t, vn) }, \ + { "ve", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_hil_gps_t, ve) }, \ + { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ + { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ + { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ + } \ +} + + +/** + * @brief Pack a hil_gps message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame + * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_GPS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} + +/** + * @brief Pack a hil_gps 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 time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame + * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_GPS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} + +/** + * @brief Encode a hil_gps 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 hil_gps C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) +{ + return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); +} + +/** + * @brief Send a hil_gps message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude (WGS84), in degrees * 1E7 + * @param lon Longitude (WGS84), in degrees * 1E7 + * @param alt Altitude (WGS84), in meters * 1000 (positive for up) + * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 + * @param vn GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + * @param ve GPS velocity in cm/s in EAST direction in earth-fixed NED frame + * @param vd GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + * @param satellites_visible Number of satellites visible. If unknown, set to 255 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_int16_t(buf, 26, vn); + _mav_put_int16_t(buf, 28, ve); + _mav_put_int16_t(buf, 30, vd); + _mav_put_uint16_t(buf, 32, cog); + _mav_put_uint8_t(buf, 34, fix_type); + _mav_put_uint8_t(buf, 35, satellites_visible); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#else + mavlink_hil_gps_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.vn = vn; + packet.ve = ve; + packet.vd = vd; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +#endif +} + +#endif + +// MESSAGE HIL_GPS UNPACKING + + +/** + * @brief Get field time_usec from hil_gps message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_gps_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from hil_gps message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_hil_gps_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field lat from hil_gps message + * + * @return Latitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_hil_gps_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from hil_gps message + * + * @return Longitude (WGS84), in degrees * 1E7 + */ +static inline int32_t mavlink_msg_hil_gps_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from hil_gps message + * + * @return Altitude (WGS84), in meters * 1000 (positive for up) + */ +static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field eph from hil_gps message + * + * @return GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field epv from hil_gps message + * + * @return GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field vel from hil_gps message + * + * @return GPS ground speed (m/s * 100). If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_vel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field vn from hil_gps message + * + * @return GPS velocity in cm/s in NORTH direction in earth-fixed NED frame + */ +static inline int16_t mavlink_msg_hil_gps_get_vn(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 26); +} + +/** + * @brief Get field ve from hil_gps message + * + * @return GPS velocity in cm/s in EAST direction in earth-fixed NED frame + */ +static inline int16_t mavlink_msg_hil_gps_get_ve(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 28); +} + +/** + * @brief Get field vd from hil_gps message + * + * @return GPS velocity in cm/s in DOWN direction in earth-fixed NED frame + */ +static inline int16_t mavlink_msg_hil_gps_get_vd(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 30); +} + +/** + * @brief Get field cog from hil_gps message + * + * @return Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + */ +static inline uint16_t mavlink_msg_hil_gps_get_cog(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field satellites_visible from hil_gps message + * + * @return Number of satellites visible. If unknown, set to 255 + */ +static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Decode a hil_gps message into a struct + * + * @param msg The message to decode + * @param hil_gps C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_gps_decode(const mavlink_message_t* msg, mavlink_hil_gps_t* hil_gps) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_gps->time_usec = mavlink_msg_hil_gps_get_time_usec(msg); + hil_gps->lat = mavlink_msg_hil_gps_get_lat(msg); + hil_gps->lon = mavlink_msg_hil_gps_get_lon(msg); + hil_gps->alt = mavlink_msg_hil_gps_get_alt(msg); + hil_gps->eph = mavlink_msg_hil_gps_get_eph(msg); + hil_gps->epv = mavlink_msg_hil_gps_get_epv(msg); + hil_gps->vel = mavlink_msg_hil_gps_get_vel(msg); + hil_gps->vn = mavlink_msg_hil_gps_get_vn(msg); + hil_gps->ve = mavlink_msg_hil_gps_get_ve(msg); + hil_gps->vd = mavlink_msg_hil_gps_get_vd(msg); + hil_gps->cog = mavlink_msg_hil_gps_get_cog(msg); + hil_gps->fix_type = mavlink_msg_hil_gps_get_fix_type(msg); + hil_gps->satellites_visible = mavlink_msg_hil_gps_get_satellites_visible(msg); +#else + memcpy(hil_gps, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_GPS_LEN); +#endif +} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_optical_flow.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_optical_flow.h new file mode 100644 index 000000000..13e13d47c --- /dev/null +++ b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_optical_flow.h @@ -0,0 +1,317 @@ +// MESSAGE HIL_OPTICAL_FLOW PACKING + +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW 114 + +typedef struct __mavlink_hil_optical_flow_t +{ + uint64_t time_usec; ///< Timestamp (UNIX) + float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated + float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated + float ground_distance; ///< Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + int16_t flow_x; ///< Flow in pixels in x-sensor direction + int16_t flow_y; ///< Flow in pixels in y-sensor direction + uint8_t sensor_id; ///< Sensor ID + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality +} mavlink_hil_optical_flow_t; + +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN 26 +#define MAVLINK_MSG_ID_114_LEN 26 + +#define MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC 119 +#define MAVLINK_MSG_ID_114_CRC 119 + + + +#define MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW { \ + "HIL_OPTICAL_FLOW", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_optical_flow_t, time_usec) }, \ + { "flow_comp_m_x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_x) }, \ + { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_optical_flow_t, flow_comp_m_y) }, \ + { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_optical_flow_t, ground_distance) }, \ + { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_hil_optical_flow_t, flow_x) }, \ + { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_hil_optical_flow_t, flow_y) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_hil_optical_flow_t, sensor_id) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_hil_optical_flow_t, quality) }, \ + } \ +} + + +/** + * @brief Pack a hil_optical_flow message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} + +/** + * @brief Pack a hil_optical_flow 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 time_usec Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_OPTICAL_FLOW; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} + +/** + * @brief Encode a hil_optical_flow 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 hil_optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_optical_flow_t* hil_optical_flow) +{ + return mavlink_msg_hil_optical_flow_pack(system_id, component_id, msg, hil_optical_flow->time_usec, hil_optical_flow->sensor_id, hil_optical_flow->flow_x, hil_optical_flow->flow_y, hil_optical_flow->flow_comp_m_x, hil_optical_flow->flow_comp_m_y, hil_optical_flow->quality, hil_optical_flow->ground_distance); +} + +/** + * @brief Send a hil_optical_flow message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (UNIX) + * @param sensor_id Sensor ID + * @param flow_x Flow in pixels in x-sensor direction + * @param flow_y Flow in pixels in y-sensor direction + * @param flow_comp_m_x Flow in meters in x-sensor direction, angular-speed compensated + * @param flow_comp_m_y Flow in meters in y-sensor direction, angular-speed compensated + * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, flow_comp_m_x); + _mav_put_float(buf, 12, flow_comp_m_y); + _mav_put_float(buf, 16, ground_distance); + _mav_put_int16_t(buf, 20, flow_x); + _mav_put_int16_t(buf, 22, flow_y); + _mav_put_uint8_t(buf, 24, sensor_id); + _mav_put_uint8_t(buf, 25, quality); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#else + mavlink_hil_optical_flow_t packet; + packet.time_usec = time_usec; + packet.flow_comp_m_x = flow_comp_m_x; + packet.flow_comp_m_y = flow_comp_m_y; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +#endif +} + +#endif + +// MESSAGE HIL_OPTICAL_FLOW UNPACKING + + +/** + * @brief Get field time_usec from hil_optical_flow message + * + * @return Timestamp (UNIX) + */ +static inline uint64_t mavlink_msg_hil_optical_flow_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_id from hil_optical_flow message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_hil_optical_flow_get_sensor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field flow_x from hil_optical_flow message + * + * @return Flow in pixels in x-sensor direction + */ +static inline int16_t mavlink_msg_hil_optical_flow_get_flow_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field flow_y from hil_optical_flow message + * + * @return Flow in pixels in y-sensor direction + */ +static inline int16_t mavlink_msg_hil_optical_flow_get_flow_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field flow_comp_m_x from hil_optical_flow message + * + * @return Flow in meters in x-sensor direction, angular-speed compensated + */ +static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field flow_comp_m_y from hil_optical_flow message + * + * @return Flow in meters in y-sensor direction, angular-speed compensated + */ +static inline float mavlink_msg_hil_optical_flow_get_flow_comp_m_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field quality from hil_optical_flow message + * + * @return Optical flow quality / confidence. 0: bad, 255: maximum quality + */ +static inline uint8_t mavlink_msg_hil_optical_flow_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Get field ground_distance from hil_optical_flow message + * + * @return Ground distance in meters. Positive value: distance known. Negative value: Unknown distance + */ +static inline float mavlink_msg_hil_optical_flow_get_ground_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a hil_optical_flow message into a struct + * + * @param msg The message to decode + * @param hil_optical_flow C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_optical_flow_decode(const mavlink_message_t* msg, mavlink_hil_optical_flow_t* hil_optical_flow) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_optical_flow->time_usec = mavlink_msg_hil_optical_flow_get_time_usec(msg); + hil_optical_flow->flow_comp_m_x = mavlink_msg_hil_optical_flow_get_flow_comp_m_x(msg); + hil_optical_flow->flow_comp_m_y = mavlink_msg_hil_optical_flow_get_flow_comp_m_y(msg); + hil_optical_flow->ground_distance = mavlink_msg_hil_optical_flow_get_ground_distance(msg); + hil_optical_flow->flow_x = mavlink_msg_hil_optical_flow_get_flow_x(msg); + hil_optical_flow->flow_y = mavlink_msg_hil_optical_flow_get_flow_y(msg); + hil_optical_flow->sensor_id = mavlink_msg_hil_optical_flow_get_sensor_id(msg); + hil_optical_flow->quality = mavlink_msg_hil_optical_flow_get_quality(msg); +#else + memcpy(hil_optical_flow, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_OPTICAL_FLOW_LEN); +#endif +} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_sensor.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_sensor.h new file mode 100644 index 000000000..422e55adc --- /dev/null +++ b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_sensor.h @@ -0,0 +1,471 @@ +// MESSAGE HIL_SENSOR PACKING + +#define MAVLINK_MSG_ID_HIL_SENSOR 107 + +typedef struct __mavlink_hil_sensor_t +{ + uint64_t time_usec; ///< Timestamp (microseconds, synced to UNIX time or since system boot) + float xacc; ///< X acceleration (m/s^2) + float yacc; ///< Y acceleration (m/s^2) + float zacc; ///< Z acceleration (m/s^2) + float xgyro; ///< Angular speed around X axis in body frame (rad / sec) + float ygyro; ///< Angular speed around Y axis in body frame (rad / sec) + float zgyro; ///< Angular speed around Z axis in body frame (rad / sec) + float xmag; ///< X Magnetic field (Gauss) + float ymag; ///< Y Magnetic field (Gauss) + float zmag; ///< Z Magnetic field (Gauss) + float abs_pressure; ///< Absolute pressure in millibar + float diff_pressure; ///< Differential pressure (airspeed) in millibar + float pressure_alt; ///< Altitude calculated from pressure + float temperature; ///< Temperature in degrees celsius + uint32_t fields_updated; ///< Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature +} mavlink_hil_sensor_t; + +#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 64 +#define MAVLINK_MSG_ID_107_LEN 64 + +#define MAVLINK_MSG_ID_HIL_SENSOR_CRC 108 +#define MAVLINK_MSG_ID_107_CRC 108 + + + +#define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ + "HIL_SENSOR", \ + 15, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ + { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_sensor_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_sensor_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_sensor_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_sensor_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_sensor_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_hil_sensor_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_hil_sensor_t, zmag) }, \ + { "abs_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_hil_sensor_t, abs_pressure) }, \ + { "diff_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_hil_sensor_t, diff_pressure) }, \ + { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ + { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ + { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ + } \ +} + + +/** + * @brief Pack a hil_sensor message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis in body frame (rad / sec) + * @param ygyro Angular speed around Y axis in body frame (rad / sec) + * @param zgyro Angular speed around Z axis in body frame (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure (airspeed) in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} + +/** + * @brief Pack a hil_sensor 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 time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis in body frame (rad / sec) + * @param ygyro Angular speed around Y axis in body frame (rad / sec) + * @param zgyro Angular speed around Z axis in body frame (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure (airspeed) in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_SENSOR; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} + +/** + * @brief Encode a hil_sensor 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 hil_sensor C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) +{ + return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); +} + +/** + * @brief Send a hil_sensor message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds, synced to UNIX time or since system boot) + * @param xacc X acceleration (m/s^2) + * @param yacc Y acceleration (m/s^2) + * @param zacc Z acceleration (m/s^2) + * @param xgyro Angular speed around X axis in body frame (rad / sec) + * @param ygyro Angular speed around Y axis in body frame (rad / sec) + * @param zgyro Angular speed around Z axis in body frame (rad / sec) + * @param xmag X Magnetic field (Gauss) + * @param ymag Y Magnetic field (Gauss) + * @param zmag Z Magnetic field (Gauss) + * @param abs_pressure Absolute pressure in millibar + * @param diff_pressure Differential pressure (airspeed) in millibar + * @param pressure_alt Altitude calculated from pressure + * @param temperature Temperature in degrees celsius + * @param fields_updated Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, xacc); + _mav_put_float(buf, 12, yacc); + _mav_put_float(buf, 16, zacc); + _mav_put_float(buf, 20, xgyro); + _mav_put_float(buf, 24, ygyro); + _mav_put_float(buf, 28, zgyro); + _mav_put_float(buf, 32, xmag); + _mav_put_float(buf, 36, ymag); + _mav_put_float(buf, 40, zmag); + _mav_put_float(buf, 44, abs_pressure); + _mav_put_float(buf, 48, diff_pressure); + _mav_put_float(buf, 52, pressure_alt); + _mav_put_float(buf, 56, temperature); + _mav_put_uint32_t(buf, 60, fields_updated); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#else + mavlink_hil_sensor_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + packet.abs_pressure = abs_pressure; + packet.diff_pressure = diff_pressure; + packet.pressure_alt = pressure_alt; + packet.temperature = temperature; + packet.fields_updated = fields_updated; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +#endif +} + +#endif + +// MESSAGE HIL_SENSOR UNPACKING + + +/** + * @brief Get field time_usec from hil_sensor message + * + * @return Timestamp (microseconds, synced to UNIX time or since system boot) + */ +static inline uint64_t mavlink_msg_hil_sensor_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field xacc from hil_sensor message + * + * @return X acceleration (m/s^2) + */ +static inline float mavlink_msg_hil_sensor_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yacc from hil_sensor message + * + * @return Y acceleration (m/s^2) + */ +static inline float mavlink_msg_hil_sensor_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field zacc from hil_sensor message + * + * @return Z acceleration (m/s^2) + */ +static inline float mavlink_msg_hil_sensor_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field xgyro from hil_sensor message + * + * @return Angular speed around X axis in body frame (rad / sec) + */ +static inline float mavlink_msg_hil_sensor_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field ygyro from hil_sensor message + * + * @return Angular speed around Y axis in body frame (rad / sec) + */ +static inline float mavlink_msg_hil_sensor_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field zgyro from hil_sensor message + * + * @return Angular speed around Z axis in body frame (rad / sec) + */ +static inline float mavlink_msg_hil_sensor_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field xmag from hil_sensor message + * + * @return X Magnetic field (Gauss) + */ +static inline float mavlink_msg_hil_sensor_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field ymag from hil_sensor message + * + * @return Y Magnetic field (Gauss) + */ +static inline float mavlink_msg_hil_sensor_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field zmag from hil_sensor message + * + * @return Z Magnetic field (Gauss) + */ +static inline float mavlink_msg_hil_sensor_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field abs_pressure from hil_sensor message + * + * @return Absolute pressure in millibar + */ +static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field diff_pressure from hil_sensor message + * + * @return Differential pressure (airspeed) in millibar + */ +static inline float mavlink_msg_hil_sensor_get_diff_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field pressure_alt from hil_sensor message + * + * @return Altitude calculated from pressure + */ +static inline float mavlink_msg_hil_sensor_get_pressure_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field temperature from hil_sensor message + * + * @return Temperature in degrees celsius + */ +static inline float mavlink_msg_hil_sensor_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field fields_updated from hil_sensor message + * + * @return Bitmask for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + */ +static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 60); +} + +/** + * @brief Decode a hil_sensor message into a struct + * + * @param msg The message to decode + * @param hil_sensor C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_sensor_decode(const mavlink_message_t* msg, mavlink_hil_sensor_t* hil_sensor) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_sensor->time_usec = mavlink_msg_hil_sensor_get_time_usec(msg); + hil_sensor->xacc = mavlink_msg_hil_sensor_get_xacc(msg); + hil_sensor->yacc = mavlink_msg_hil_sensor_get_yacc(msg); + hil_sensor->zacc = mavlink_msg_hil_sensor_get_zacc(msg); + hil_sensor->xgyro = mavlink_msg_hil_sensor_get_xgyro(msg); + hil_sensor->ygyro = mavlink_msg_hil_sensor_get_ygyro(msg); + hil_sensor->zgyro = mavlink_msg_hil_sensor_get_zgyro(msg); + hil_sensor->xmag = mavlink_msg_hil_sensor_get_xmag(msg); + hil_sensor->ymag = mavlink_msg_hil_sensor_get_ymag(msg); + hil_sensor->zmag = mavlink_msg_hil_sensor_get_zmag(msg); + hil_sensor->abs_pressure = mavlink_msg_hil_sensor_get_abs_pressure(msg); + hil_sensor->diff_pressure = mavlink_msg_hil_sensor_get_diff_pressure(msg); + hil_sensor->pressure_alt = mavlink_msg_hil_sensor_get_pressure_alt(msg); + hil_sensor->temperature = mavlink_msg_hil_sensor_get_temperature(msg); + hil_sensor->fields_updated = mavlink_msg_hil_sensor_get_fields_updated(msg); +#else + memcpy(hil_sensor, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_SENSOR_LEN); +#endif +} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_state_quaternion.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_state_quaternion.h new file mode 100644 index 000000000..0474e64a2 --- /dev/null +++ b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_hil_state_quaternion.h @@ -0,0 +1,487 @@ +// MESSAGE HIL_STATE_QUATERNION PACKING + +#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION 115 + +typedef struct __mavlink_hil_state_quaternion_t +{ + uint64_t time_usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float attitude_quaternion[4]; ///< Vehicle attitude expressed as normalized quaternion + float rollspeed; ///< Body frame roll / phi angular speed (rad/s) + float pitchspeed; ///< Body frame pitch / theta angular speed (rad/s) + float yawspeed; ///< Body frame yaw / psi angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + uint16_t ind_airspeed; ///< Indicated airspeed, expressed as m/s * 100 + uint16_t true_airspeed; ///< True airspeed, expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) +} mavlink_hil_state_quaternion_t; + +#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN 64 +#define MAVLINK_MSG_ID_115_LEN 64 + +#define MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC 4 +#define MAVLINK_MSG_ID_115_CRC 4 + +#define MAVLINK_MSG_HIL_STATE_QUATERNION_FIELD_ATTITUDE_QUATERNION_LEN 4 + +#define MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION { \ + "HIL_STATE_QUATERNION", \ + 16, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_quaternion_t, time_usec) }, \ + { "attitude_quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_hil_state_quaternion_t, attitude_quaternion) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_quaternion_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_quaternion_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_hil_state_quaternion_t, yawspeed) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_quaternion_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_quaternion_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_hil_state_quaternion_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_quaternion_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_quaternion_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_quaternion_t, vz) }, \ + { "ind_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 54, offsetof(mavlink_hil_state_quaternion_t, ind_airspeed) }, \ + { "true_airspeed", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_hil_state_quaternion_t, true_airspeed) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 58, offsetof(mavlink_hil_state_quaternion_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 60, offsetof(mavlink_hil_state_quaternion_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 62, offsetof(mavlink_hil_state_quaternion_t, zacc) }, \ + } \ +} + + +/** + * @brief Pack a hil_state_quaternion message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 + * @param true_airspeed True airspeed, expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} + +/** + * @brief Pack a hil_state_quaternion 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 time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 + * @param true_airspeed True airspeed, expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,const float *attitude_quaternion,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,uint16_t ind_airspeed,uint16_t true_airspeed,int16_t xacc,int16_t yacc,int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE_QUATERNION; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} + +/** + * @brief Encode a hil_state_quaternion 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 hil_state_quaternion C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ + return mavlink_msg_hil_state_quaternion_pack(system_id, component_id, msg, hil_state_quaternion->time_usec, hil_state_quaternion->attitude_quaternion, hil_state_quaternion->rollspeed, hil_state_quaternion->pitchspeed, hil_state_quaternion->yawspeed, hil_state_quaternion->lat, hil_state_quaternion->lon, hil_state_quaternion->alt, hil_state_quaternion->vx, hil_state_quaternion->vy, hil_state_quaternion->vz, hil_state_quaternion->ind_airspeed, hil_state_quaternion->true_airspeed, hil_state_quaternion->xacc, hil_state_quaternion->yacc, hil_state_quaternion->zacc); +} + +/** + * @brief Send a hil_state_quaternion message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param attitude_quaternion Vehicle attitude expressed as normalized quaternion + * @param rollspeed Body frame roll / phi angular speed (rad/s) + * @param pitchspeed Body frame pitch / theta angular speed (rad/s) + * @param yawspeed Body frame yaw / psi angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param ind_airspeed Indicated airspeed, expressed as m/s * 100 + * @param true_airspeed True airspeed, expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_state_quaternion_send(mavlink_channel_t chan, uint64_t time_usec, const float *attitude_quaternion, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t ind_airspeed, uint16_t true_airspeed, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 24, rollspeed); + _mav_put_float(buf, 28, pitchspeed); + _mav_put_float(buf, 32, yawspeed); + _mav_put_int32_t(buf, 36, lat); + _mav_put_int32_t(buf, 40, lon); + _mav_put_int32_t(buf, 44, alt); + _mav_put_int16_t(buf, 48, vx); + _mav_put_int16_t(buf, 50, vy); + _mav_put_int16_t(buf, 52, vz); + _mav_put_uint16_t(buf, 54, ind_airspeed); + _mav_put_uint16_t(buf, 56, true_airspeed); + _mav_put_int16_t(buf, 58, xacc); + _mav_put_int16_t(buf, 60, yacc); + _mav_put_int16_t(buf, 62, zacc); + _mav_put_float_array(buf, 8, attitude_quaternion, 4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, buf, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#else + mavlink_hil_state_quaternion_t packet; + packet.time_usec = time_usec; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.ind_airspeed = ind_airspeed; + packet.true_airspeed = true_airspeed; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + mav_array_memcpy(packet.attitude_quaternion, attitude_quaternion, sizeof(float)*4); +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +#endif +} + +#endif + +// MESSAGE HIL_STATE_QUATERNION UNPACKING + + +/** + * @brief Get field time_usec from hil_state_quaternion message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_state_quaternion_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field attitude_quaternion from hil_state_quaternion message + * + * @return Vehicle attitude expressed as normalized quaternion + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_get_attitude_quaternion(const mavlink_message_t* msg, float *attitude_quaternion) +{ + return _MAV_RETURN_float_array(msg, attitude_quaternion, 4, 8); +} + +/** + * @brief Get field rollspeed from hil_state_quaternion message + * + * @return Body frame roll / phi angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_quaternion_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field pitchspeed from hil_state_quaternion message + * + * @return Body frame pitch / theta angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_quaternion_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field yawspeed from hil_state_quaternion message + * + * @return Body frame yaw / psi angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_quaternion_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field lat from hil_state_quaternion message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_quaternion_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field lon from hil_state_quaternion message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_quaternion_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 40); +} + +/** + * @brief Get field alt from hil_state_quaternion message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_hil_state_quaternion_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 44); +} + +/** + * @brief Get field vx from hil_state_quaternion message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 48); +} + +/** + * @brief Get field vy from hil_state_quaternion message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 50); +} + +/** + * @brief Get field vz from hil_state_quaternion message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 52); +} + +/** + * @brief Get field ind_airspeed from hil_state_quaternion message + * + * @return Indicated airspeed, expressed as m/s * 100 + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_get_ind_airspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 54); +} + +/** + * @brief Get field true_airspeed from hil_state_quaternion message + * + * @return True airspeed, expressed as m/s * 100 + */ +static inline uint16_t mavlink_msg_hil_state_quaternion_get_true_airspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 56); +} + +/** + * @brief Get field xacc from hil_state_quaternion message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 58); +} + +/** + * @brief Get field yacc from hil_state_quaternion message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 60); +} + +/** + * @brief Get field zacc from hil_state_quaternion message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_quaternion_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 62); +} + +/** + * @brief Decode a hil_state_quaternion message into a struct + * + * @param msg The message to decode + * @param hil_state_quaternion C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_state_quaternion_decode(const mavlink_message_t* msg, mavlink_hil_state_quaternion_t* hil_state_quaternion) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_state_quaternion->time_usec = mavlink_msg_hil_state_quaternion_get_time_usec(msg); + mavlink_msg_hil_state_quaternion_get_attitude_quaternion(msg, hil_state_quaternion->attitude_quaternion); + hil_state_quaternion->rollspeed = mavlink_msg_hil_state_quaternion_get_rollspeed(msg); + hil_state_quaternion->pitchspeed = mavlink_msg_hil_state_quaternion_get_pitchspeed(msg); + hil_state_quaternion->yawspeed = mavlink_msg_hil_state_quaternion_get_yawspeed(msg); + hil_state_quaternion->lat = mavlink_msg_hil_state_quaternion_get_lat(msg); + hil_state_quaternion->lon = mavlink_msg_hil_state_quaternion_get_lon(msg); + hil_state_quaternion->alt = mavlink_msg_hil_state_quaternion_get_alt(msg); + hil_state_quaternion->vx = mavlink_msg_hil_state_quaternion_get_vx(msg); + hil_state_quaternion->vy = mavlink_msg_hil_state_quaternion_get_vy(msg); + hil_state_quaternion->vz = mavlink_msg_hil_state_quaternion_get_vz(msg); + hil_state_quaternion->ind_airspeed = mavlink_msg_hil_state_quaternion_get_ind_airspeed(msg); + hil_state_quaternion->true_airspeed = mavlink_msg_hil_state_quaternion_get_true_airspeed(msg); + hil_state_quaternion->xacc = mavlink_msg_hil_state_quaternion_get_xacc(msg); + hil_state_quaternion->yacc = mavlink_msg_hil_state_quaternion_get_yacc(msg); + hil_state_quaternion->zacc = mavlink_msg_hil_state_quaternion_get_zacc(msg); +#else + memcpy(hil_state_quaternion, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_HIL_STATE_QUATERNION_LEN); +#endif +} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position.h deleted file mode 100644 index e0a90edae..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position.h +++ /dev/null @@ -1,276 +0,0 @@ -// MESSAGE LOCAL_POSITION PACKING - -#define MAVLINK_MSG_ID_LOCAL_POSITION 31 - -typedef struct __mavlink_local_position_t -{ - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - float vx; ///< X Speed - float vy; ///< Y Speed - float vz; ///< Z Speed -} mavlink_local_position_t; - -#define MAVLINK_MSG_ID_LOCAL_POSITION_LEN 32 -#define MAVLINK_MSG_ID_31_LEN 32 - - - -#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION { \ - "LOCAL_POSITION", \ - 7, \ - { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_t, usec) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_t, z) }, \ - { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_t, vx) }, \ - { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_t, vy) }, \ - { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_t, vz) }, \ - } \ -} - - -/** - * @brief Pack a local_position message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_local_position_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - return mavlink_finalize_message(msg, system_id, component_id, 32, 126); -} - -/** - * @brief Pack a local_position 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 usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float vx,float vy,float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - memcpy(_MAV_PAYLOAD(msg), buf, 32); -#else - mavlink_local_position_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - memcpy(_MAV_PAYLOAD(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 126); -} - -/** - * @brief Encode a local_position 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 local_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_t* local_position) -{ - return mavlink_msg_local_position_pack(system_id, component_id, msg, local_position->usec, local_position->x, local_position->y, local_position->z, local_position->vx, local_position->vy, local_position->vz); -} - -/** - * @brief Send a local_position message - * @param chan MAVLink channel to send the message - * - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - _mav_put_uint64_t(buf, 0, usec); - _mav_put_float(buf, 8, x); - _mav_put_float(buf, 12, y); - _mav_put_float(buf, 16, z); - _mav_put_float(buf, 20, vx); - _mav_put_float(buf, 24, vy); - _mav_put_float(buf, 28, vz); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION, buf, 32, 126); -#else - mavlink_local_position_t packet; - packet.usec = usec; - packet.x = x; - packet.y = y; - packet.z = z; - packet.vx = vx; - packet.vy = vy; - packet.vz = vz; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION, (const char *)&packet, 32, 126); -#endif -} - -#endif - -// MESSAGE LOCAL_POSITION UNPACKING - - -/** - * @brief Get field usec from local_position message - * - * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) - */ -static inline uint64_t mavlink_msg_local_position_get_usec(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint64_t(msg, 0); -} - -/** - * @brief Get field x from local_position message - * - * @return X Position - */ -static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field y from local_position message - * - * @return Y Position - */ -static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field z from local_position message - * - * @return Z Position - */ -static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field vx from local_position message - * - * @return X Speed - */ -static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field vy from local_position message - * - * @return Y Speed - */ -static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Get field vz from local_position message - * - * @return Z Speed - */ -static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 28); -} - -/** - * @brief Decode a local_position message into a struct - * - * @param msg The message to decode - * @param local_position C-struct to decode the message contents into - */ -static inline void mavlink_msg_local_position_decode(const mavlink_message_t* msg, mavlink_local_position_t* local_position) -{ -#if MAVLINK_NEED_BYTE_SWAP - local_position->usec = mavlink_msg_local_position_get_usec(msg); - local_position->x = mavlink_msg_local_position_get_x(msg); - local_position->y = mavlink_msg_local_position_get_y(msg); - local_position->z = mavlink_msg_local_position_get_z(msg); - local_position->vx = mavlink_msg_local_position_get_vx(msg); - local_position->vy = mavlink_msg_local_position_get_vy(msg); - local_position->vz = mavlink_msg_local_position_get_vz(msg); -#else - memcpy(local_position, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position_setpoint_set.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position_setpoint_set.h deleted file mode 100644 index a3183eb69..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_local_position_setpoint_set.h +++ /dev/null @@ -1,254 +0,0 @@ -// MESSAGE LOCAL_POSITION_SETPOINT_SET PACKING - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 - -typedef struct __mavlink_local_position_setpoint_set_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_local_position_setpoint_set_t; - -#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN 18 -#define MAVLINK_MSG_ID_50_LEN 18 - - - -#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET { \ - "LOCAL_POSITION_SETPOINT_SET", \ - 6, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_set_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_set_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_set_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_set_t, yaw) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_local_position_setpoint_set_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_local_position_setpoint_set_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a local_position_setpoint_set 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 x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 18); -#else - mavlink_local_position_setpoint_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - return mavlink_finalize_message(msg, system_id, component_id, 18, 73); -} - -/** - * @brief Pack a local_position_setpoint_set 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 target_system System ID - * @param target_component Component ID - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_local_position_setpoint_set_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,float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 18); -#else - mavlink_local_position_setpoint_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 18); -#endif - - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 73); -} - -/** - * @brief Encode a local_position_setpoint_set 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 local_position_setpoint_set C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_setpoint_set_t* local_position_setpoint_set) -{ - return mavlink_msg_local_position_setpoint_set_pack(system_id, component_id, msg, local_position_setpoint_set->target_system, local_position_setpoint_set->target_component, local_position_setpoint_set->x, local_position_setpoint_set->y, local_position_setpoint_set->z, local_position_setpoint_set->yaw); -} - -/** - * @brief Send a local_position_setpoint_set message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[18]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - _mav_put_uint8_t(buf, 16, target_system); - _mav_put_uint8_t(buf, 17, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, buf, 18, 73); -#else - mavlink_local_position_setpoint_set_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, (const char *)&packet, 18, 73); -#endif -} - -#endif - -// MESSAGE LOCAL_POSITION_SETPOINT_SET UNPACKING - - -/** - * @brief Get field target_system from local_position_setpoint_set message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 16); -} - -/** - * @brief Get field target_component from local_position_setpoint_set message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 17); -} - -/** - * @brief Get field x from local_position_setpoint_set message - * - * @return x position - */ -static inline float mavlink_msg_local_position_setpoint_set_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from local_position_setpoint_set message - * - * @return y position - */ -static inline float mavlink_msg_local_position_setpoint_set_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from local_position_setpoint_set message - * - * @return z position - */ -static inline float mavlink_msg_local_position_setpoint_set_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from local_position_setpoint_set message - * - * @return Desired yaw angle - */ -static inline float mavlink_msg_local_position_setpoint_set_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a local_position_setpoint_set message into a struct - * - * @param msg The message to decode - * @param local_position_setpoint_set C-struct to decode the message contents into - */ -static inline void mavlink_msg_local_position_setpoint_set_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_set_t* local_position_setpoint_set) -{ -#if MAVLINK_NEED_BYTE_SWAP - local_position_setpoint_set->x = mavlink_msg_local_position_setpoint_set_get_x(msg); - local_position_setpoint_set->y = mavlink_msg_local_position_setpoint_set_get_y(msg); - local_position_setpoint_set->z = mavlink_msg_local_position_setpoint_set_get_z(msg); - local_position_setpoint_set->yaw = mavlink_msg_local_position_setpoint_set_get_yaw(msg); - local_position_setpoint_set->target_system = mavlink_msg_local_position_setpoint_set_get_target_system(msg); - local_position_setpoint_set->target_component = mavlink_msg_local_position_setpoint_set_get_target_component(msg); -#else - memcpy(local_position_setpoint_set, _MAV_PAYLOAD(msg), 18); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_object_detection_event.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_object_detection_event.h deleted file mode 100644 index 3ee70ee87..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_object_detection_event.h +++ /dev/null @@ -1,270 +0,0 @@ -// MESSAGE OBJECT_DETECTION_EVENT PACKING - -#define MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT 140 - -typedef struct __mavlink_object_detection_event_t -{ - uint32_t time; ///< Timestamp in milliseconds since system boot - float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front - float distance; ///< Ground distance in meters - uint16_t object_id; ///< Object ID - uint8_t type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal - char name[20]; ///< Name of the object as defined by the detector - uint8_t quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence -} mavlink_object_detection_event_t; - -#define MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT_LEN 36 -#define MAVLINK_MSG_ID_140_LEN 36 - -#define MAVLINK_MSG_OBJECT_DETECTION_EVENT_FIELD_NAME_LEN 20 - -#define MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT { \ - "OBJECT_DETECTION_EVENT", \ - 7, \ - { { "time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_object_detection_event_t, time) }, \ - { "bearing", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_object_detection_event_t, bearing) }, \ - { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_object_detection_event_t, distance) }, \ - { "object_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_object_detection_event_t, object_id) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_object_detection_event_t, type) }, \ - { "name", NULL, MAVLINK_TYPE_CHAR, 20, 15, offsetof(mavlink_object_detection_event_t, name) }, \ - { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_object_detection_event_t, quality) }, \ - } \ -} - - -/** - * @brief Pack a object_detection_event message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param time Timestamp in milliseconds since system boot - * @param object_id Object ID - * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal - * @param name Name of the object as defined by the detector - * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence - * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front - * @param distance Ground distance in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_object_detection_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time, uint16_t object_id, uint8_t type, const char *name, uint8_t quality, float bearing, float distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; - _mav_put_uint32_t(buf, 0, time); - _mav_put_float(buf, 4, bearing); - _mav_put_float(buf, 8, distance); - _mav_put_uint16_t(buf, 12, object_id); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 35, quality); - _mav_put_char_array(buf, 15, name, 20); - memcpy(_MAV_PAYLOAD(msg), buf, 36); -#else - mavlink_object_detection_event_t packet; - packet.time = time; - packet.bearing = bearing; - packet.distance = distance; - packet.object_id = object_id; - packet.type = type; - packet.quality = quality; - mav_array_memcpy(packet.name, name, sizeof(char)*20); - memcpy(_MAV_PAYLOAD(msg), &packet, 36); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; - return mavlink_finalize_message(msg, system_id, component_id, 36, 179); -} - -/** - * @brief Pack a object_detection_event 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 time Timestamp in milliseconds since system boot - * @param object_id Object ID - * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal - * @param name Name of the object as defined by the detector - * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence - * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front - * @param distance Ground distance in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_object_detection_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint32_t time,uint16_t object_id,uint8_t type,const char *name,uint8_t quality,float bearing,float distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; - _mav_put_uint32_t(buf, 0, time); - _mav_put_float(buf, 4, bearing); - _mav_put_float(buf, 8, distance); - _mav_put_uint16_t(buf, 12, object_id); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 35, quality); - _mav_put_char_array(buf, 15, name, 20); - memcpy(_MAV_PAYLOAD(msg), buf, 36); -#else - mavlink_object_detection_event_t packet; - packet.time = time; - packet.bearing = bearing; - packet.distance = distance; - packet.object_id = object_id; - packet.type = type; - packet.quality = quality; - mav_array_memcpy(packet.name, name, sizeof(char)*20); - memcpy(_MAV_PAYLOAD(msg), &packet, 36); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 179); -} - -/** - * @brief Encode a object_detection_event 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 object_detection_event C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_object_detection_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_object_detection_event_t* object_detection_event) -{ - return mavlink_msg_object_detection_event_pack(system_id, component_id, msg, object_detection_event->time, object_detection_event->object_id, object_detection_event->type, object_detection_event->name, object_detection_event->quality, object_detection_event->bearing, object_detection_event->distance); -} - -/** - * @brief Send a object_detection_event message - * @param chan MAVLink channel to send the message - * - * @param time Timestamp in milliseconds since system boot - * @param object_id Object ID - * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal - * @param name Name of the object as defined by the detector - * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence - * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front - * @param distance Ground distance in meters - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_object_detection_event_send(mavlink_channel_t chan, uint32_t time, uint16_t object_id, uint8_t type, const char *name, uint8_t quality, float bearing, float distance) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; - _mav_put_uint32_t(buf, 0, time); - _mav_put_float(buf, 4, bearing); - _mav_put_float(buf, 8, distance); - _mav_put_uint16_t(buf, 12, object_id); - _mav_put_uint8_t(buf, 14, type); - _mav_put_uint8_t(buf, 35, quality); - _mav_put_char_array(buf, 15, name, 20); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT, buf, 36, 179); -#else - mavlink_object_detection_event_t packet; - packet.time = time; - packet.bearing = bearing; - packet.distance = distance; - packet.object_id = object_id; - packet.type = type; - packet.quality = quality; - mav_array_memcpy(packet.name, name, sizeof(char)*20); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT, (const char *)&packet, 36, 179); -#endif -} - -#endif - -// MESSAGE OBJECT_DETECTION_EVENT UNPACKING - - -/** - * @brief Get field time from object_detection_event message - * - * @return Timestamp in milliseconds since system boot - */ -static inline uint32_t mavlink_msg_object_detection_event_get_time(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Get field object_id from object_detection_event message - * - * @return Object ID - */ -static inline uint16_t mavlink_msg_object_detection_event_get_object_id(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 12); -} - -/** - * @brief Get field type from object_detection_event message - * - * @return Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal - */ -static inline uint8_t mavlink_msg_object_detection_event_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 14); -} - -/** - * @brief Get field name from object_detection_event message - * - * @return Name of the object as defined by the detector - */ -static inline uint16_t mavlink_msg_object_detection_event_get_name(const mavlink_message_t* msg, char *name) -{ - return _MAV_RETURN_char_array(msg, name, 20, 15); -} - -/** - * @brief Get field quality from object_detection_event message - * - * @return Detection quality / confidence. 0: bad, 255: maximum confidence - */ -static inline uint8_t mavlink_msg_object_detection_event_get_quality(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 35); -} - -/** - * @brief Get field bearing from object_detection_event message - * - * @return Angle of the object with respect to the body frame in NED coordinates in radians. 0: front - */ -static inline float mavlink_msg_object_detection_event_get_bearing(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field distance from object_detection_event message - * - * @return Ground distance in meters - */ -static inline float mavlink_msg_object_detection_event_get_distance(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a object_detection_event message into a struct - * - * @param msg The message to decode - * @param object_detection_event C-struct to decode the message contents into - */ -static inline void mavlink_msg_object_detection_event_decode(const mavlink_message_t* msg, mavlink_object_detection_event_t* object_detection_event) -{ -#if MAVLINK_NEED_BYTE_SWAP - object_detection_event->time = mavlink_msg_object_detection_event_get_time(msg); - object_detection_event->bearing = mavlink_msg_object_detection_event_get_bearing(msg); - object_detection_event->distance = mavlink_msg_object_detection_event_get_distance(msg); - object_detection_event->object_id = mavlink_msg_object_detection_event_get_object_id(msg); - object_detection_event->type = mavlink_msg_object_detection_event_get_type(msg); - mavlink_msg_object_detection_event_get_name(msg, object_detection_event->name); - object_detection_event->quality = mavlink_msg_object_detection_event_get_quality(msg); -#else - memcpy(object_detection_event, _MAV_PAYLOAD(msg), 36); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h deleted file mode 100644 index 44c0e6931..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_position_target.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE POSITION_TARGET PACKING - -#define MAVLINK_MSG_ID_POSITION_TARGET 63 - -typedef struct __mavlink_position_target_t -{ - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH -} mavlink_position_target_t; - -#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 -#define MAVLINK_MSG_ID_63_LEN 16 - - - -#define MAVLINK_MESSAGE_INFO_POSITION_TARGET { \ - "POSITION_TARGET", \ - 4, \ - { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_target_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_t, z) }, \ - { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_t, yaw) }, \ - } \ -} - - -/** - * @brief Pack a position_target message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[16]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - - memcpy(_MAV_PAYLOAD(msg), buf, 16); -#else - mavlink_position_target_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 16); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - return mavlink_finalize_message(msg, system_id, component_id, 16, 126); -} - -/** - * @brief Pack a position_target 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 x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float x,float y,float z,float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[16]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - - memcpy(_MAV_PAYLOAD(msg), buf, 16); -#else - mavlink_position_target_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - - memcpy(_MAV_PAYLOAD(msg), &packet, 16); -#endif - - msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 126); -} - -/** - * @brief Encode a position_target 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 position_target C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_t* position_target) -{ - return mavlink_msg_position_target_pack(system_id, component_id, msg, position_target->x, position_target->y, position_target->z, position_target->yaw); -} - -/** - * @brief Send a position_target message - * @param chan MAVLink channel to send the message - * - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[16]; - _mav_put_float(buf, 0, x); - _mav_put_float(buf, 4, y); - _mav_put_float(buf, 8, z); - _mav_put_float(buf, 12, yaw); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET, buf, 16, 126); -#else - mavlink_position_target_t packet; - packet.x = x; - packet.y = y; - packet.z = z; - packet.yaw = yaw; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET, (const char *)&packet, 16, 126); -#endif -} - -#endif - -// MESSAGE POSITION_TARGET UNPACKING - - -/** - * @brief Get field x from position_target message - * - * @return x position - */ -static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field y from position_target message - * - * @return y position - */ -static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field z from position_target message - * - * @return z position - */ -static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field yaw from position_target message - * - * @return yaw orientation in radians, 0 = NORTH - */ -static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Decode a position_target message into a struct - * - * @param msg The message to decode - * @param position_target C-struct to decode the message contents into - */ -static inline void mavlink_msg_position_target_decode(const mavlink_message_t* msg, mavlink_position_target_t* position_target) -{ -#if MAVLINK_NEED_BYTE_SWAP - position_target->x = mavlink_msg_position_target_get_x(msg); - position_target->y = mavlink_msg_position_target_get_y(msg); - position_target->z = mavlink_msg_position_target_get_z(msg); - position_target->yaw = mavlink_msg_position_target_get_yaw(msg); -#else - memcpy(position_target, _MAV_PAYLOAD(msg), 16); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_radio_status.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_radio_status.h new file mode 100644 index 000000000..06e6a5542 --- /dev/null +++ b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_radio_status.h @@ -0,0 +1,295 @@ +// MESSAGE RADIO_STATUS PACKING + +#define MAVLINK_MSG_ID_RADIO_STATUS 109 + +typedef struct __mavlink_radio_status_t +{ + uint16_t rxerrors; ///< receive errors + uint16_t fixed; ///< count of error corrected packets + uint8_t rssi; ///< local signal strength + uint8_t remrssi; ///< remote signal strength + uint8_t txbuf; ///< how full the tx buffer is as a percentage + uint8_t noise; ///< background noise level + uint8_t remnoise; ///< remote background noise level +} mavlink_radio_status_t; + +#define MAVLINK_MSG_ID_RADIO_STATUS_LEN 9 +#define MAVLINK_MSG_ID_109_LEN 9 + +#define MAVLINK_MSG_ID_RADIO_STATUS_CRC 185 +#define MAVLINK_MSG_ID_109_CRC 185 + + + +#define MAVLINK_MESSAGE_INFO_RADIO_STATUS { \ + "RADIO_STATUS", \ + 7, \ + { { "rxerrors", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_radio_status_t, rxerrors) }, \ + { "fixed", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_radio_status_t, fixed) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_radio_status_t, rssi) }, \ + { "remrssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_radio_status_t, remrssi) }, \ + { "txbuf", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_radio_status_t, txbuf) }, \ + { "noise", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_radio_status_t, noise) }, \ + { "remnoise", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_radio_status_t, remnoise) }, \ + } \ +} + + +/** + * @brief Pack a radio_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 rssi local signal strength + * @param remrssi remote signal strength + * @param txbuf how full the tx buffer is as a percentage + * @param noise background noise level + * @param remnoise remote background noise level + * @param rxerrors receive errors + * @param fixed count of error corrected packets + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} + +/** + * @brief Pack a radio_status 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 rssi local signal strength + * @param remrssi remote signal strength + * @param txbuf how full the tx buffer is as a percentage + * @param noise background noise level + * @param remnoise remote background noise level + * @param rxerrors receive errors + * @param fixed count of error corrected packets + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_radio_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t rssi,uint8_t remrssi,uint8_t txbuf,uint8_t noise,uint8_t remnoise,uint16_t rxerrors,uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO_STATUS; +#if MAVLINK_CRC_EXTRA + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} + +/** + * @brief Encode a radio_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 radio_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_radio_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_radio_status_t* radio_status) +{ + return mavlink_msg_radio_status_pack(system_id, component_id, msg, radio_status->rssi, radio_status->remrssi, radio_status->txbuf, radio_status->noise, radio_status->remnoise, radio_status->rxerrors, radio_status->fixed); +} + +/** + * @brief Send a radio_status message + * @param chan MAVLink channel to send the message + * + * @param rssi local signal strength + * @param remrssi remote signal strength + * @param txbuf how full the tx buffer is as a percentage + * @param noise background noise level + * @param remnoise remote background noise level + * @param rxerrors receive errors + * @param fixed count of error corrected packets + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_radio_status_send(mavlink_channel_t chan, uint8_t rssi, uint8_t remrssi, uint8_t txbuf, uint8_t noise, uint8_t remnoise, uint16_t rxerrors, uint16_t fixed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RADIO_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, rxerrors); + _mav_put_uint16_t(buf, 2, fixed); + _mav_put_uint8_t(buf, 4, rssi); + _mav_put_uint8_t(buf, 5, remrssi); + _mav_put_uint8_t(buf, 6, txbuf); + _mav_put_uint8_t(buf, 7, noise); + _mav_put_uint8_t(buf, 8, remnoise); + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, buf, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#else + mavlink_radio_status_t packet; + packet.rxerrors = rxerrors; + packet.fixed = fixed; + packet.rssi = rssi; + packet.remrssi = remrssi; + packet.txbuf = txbuf; + packet.noise = noise; + packet.remnoise = remnoise; + +#if MAVLINK_CRC_EXTRA + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN, MAVLINK_MSG_ID_RADIO_STATUS_CRC); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +#endif +} + +#endif + +// MESSAGE RADIO_STATUS UNPACKING + + +/** + * @brief Get field rssi from radio_status message + * + * @return local signal strength + */ +static inline uint8_t mavlink_msg_radio_status_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field remrssi from radio_status message + * + * @return remote signal strength + */ +static inline uint8_t mavlink_msg_radio_status_get_remrssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field txbuf from radio_status message + * + * @return how full the tx buffer is as a percentage + */ +static inline uint8_t mavlink_msg_radio_status_get_txbuf(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field noise from radio_status message + * + * @return background noise level + */ +static inline uint8_t mavlink_msg_radio_status_get_noise(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field remnoise from radio_status message + * + * @return remote background noise level + */ +static inline uint8_t mavlink_msg_radio_status_get_remnoise(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field rxerrors from radio_status message + * + * @return receive errors + */ +static inline uint16_t mavlink_msg_radio_status_get_rxerrors(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field fixed from radio_status message + * + * @return count of error corrected packets + */ +static inline uint16_t mavlink_msg_radio_status_get_fixed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a radio_status message into a struct + * + * @param msg The message to decode + * @param radio_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_radio_status_decode(const mavlink_message_t* msg, mavlink_radio_status_t* radio_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + radio_status->rxerrors = mavlink_msg_radio_status_get_rxerrors(msg); + radio_status->fixed = mavlink_msg_radio_status_get_fixed(msg); + radio_status->rssi = mavlink_msg_radio_status_get_rssi(msg); + radio_status->remrssi = mavlink_msg_radio_status_get_remrssi(msg); + radio_status->txbuf = mavlink_msg_radio_status_get_txbuf(msg); + radio_status->noise = mavlink_msg_radio_status_get_noise(msg); + radio_status->remnoise = mavlink_msg_radio_status_get_remnoise(msg); +#else + memcpy(radio_status, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_RADIO_STATUS_LEN); +#endif +} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_altitude.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_altitude.h deleted file mode 100644 index c7a768aa4..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_altitude.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE SET_ALTITUDE PACKING - -#define MAVLINK_MSG_ID_SET_ALTITUDE 65 - -typedef struct __mavlink_set_altitude_t -{ - uint32_t mode; ///< The new altitude in meters - uint8_t target; ///< The system setting the altitude -} mavlink_set_altitude_t; - -#define MAVLINK_MSG_ID_SET_ALTITUDE_LEN 5 -#define MAVLINK_MSG_ID_65_LEN 5 - - - -#define MAVLINK_MESSAGE_INFO_SET_ALTITUDE { \ - "SET_ALTITUDE", \ - 2, \ - { { "mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_set_altitude_t, mode) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_set_altitude_t, target) }, \ - } \ -} - - -/** - * @brief Pack a set_altitude 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 The system setting the altitude - * @param mode The new altitude in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint32_t mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_uint32_t(buf, 0, mode); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD(msg), buf, 5); -#else - mavlink_set_altitude_t packet; - packet.mode = mode; - packet.target = target; - - memcpy(_MAV_PAYLOAD(msg), &packet, 5); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - return mavlink_finalize_message(msg, system_id, component_id, 5, 54); -} - -/** - * @brief Pack a set_altitude 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 target The system setting the altitude - * @param mode The new altitude in meters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_altitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint32_t mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_uint32_t(buf, 0, mode); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD(msg), buf, 5); -#else - mavlink_set_altitude_t packet; - packet.mode = mode; - packet.target = target; - - memcpy(_MAV_PAYLOAD(msg), &packet, 5); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 54); -} - -/** - * @brief Encode a set_altitude 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 set_altitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_altitude_t* set_altitude) -{ - return mavlink_msg_set_altitude_pack(system_id, component_id, msg, set_altitude->target, set_altitude->mode); -} - -/** - * @brief Send a set_altitude message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the altitude - * @param mode The new altitude in meters - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_uint32_t(buf, 0, mode); - _mav_put_uint8_t(buf, 4, target); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ALTITUDE, buf, 5, 54); -#else - mavlink_set_altitude_t packet; - packet.mode = mode; - packet.target = target; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ALTITUDE, (const char *)&packet, 5, 54); -#endif -} - -#endif - -// MESSAGE SET_ALTITUDE UNPACKING - - -/** - * @brief Get field target from set_altitude message - * - * @return The system setting the altitude - */ -static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field mode from set_altitude message - * - * @return The new altitude in meters - */ -static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @brief Decode a set_altitude message into a struct - * - * @param msg The message to decode - * @param set_altitude C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_altitude_decode(const mavlink_message_t* msg, mavlink_set_altitude_t* set_altitude) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); - set_altitude->target = mavlink_msg_set_altitude_get_target(msg); -#else - memcpy(set_altitude, _MAV_PAYLOAD(msg), 5); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_nav_mode.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_nav_mode.h deleted file mode 100644 index 5832ecb6b..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_set_nav_mode.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE SET_NAV_MODE PACKING - -#define MAVLINK_MSG_ID_SET_NAV_MODE 12 - -typedef struct __mavlink_set_nav_mode_t -{ - uint8_t target; ///< The system setting the mode - uint8_t nav_mode; ///< The new navigation mode -} mavlink_set_nav_mode_t; - -#define MAVLINK_MSG_ID_SET_NAV_MODE_LEN 2 -#define MAVLINK_MSG_ID_12_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_SET_NAV_MODE { \ - "SET_NAV_MODE", \ - 2, \ - { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_nav_mode_t, target) }, \ - { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_nav_mode_t, nav_mode) }, \ - } \ -} - - -/** - * @brief Pack a set_nav_mode 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 The system setting the mode - * @param nav_mode The new navigation mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, nav_mode); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_set_nav_mode_t packet; - packet.target = target; - packet.nav_mode = nav_mode; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - return mavlink_finalize_message(msg, system_id, component_id, 2, 10); -} - -/** - * @brief Pack a set_nav_mode 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 target The system setting the mode - * @param nav_mode The new navigation mode - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_set_nav_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, nav_mode); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_set_nav_mode_t packet; - packet.target = target; - packet.nav_mode = nav_mode; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 10); -} - -/** - * @brief Encode a set_nav_mode 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 set_nav_mode C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_nav_mode_t* set_nav_mode) -{ - return mavlink_msg_set_nav_mode_pack(system_id, component_id, msg, set_nav_mode->target, set_nav_mode->nav_mode); -} - -/** - * @brief Send a set_nav_mode message - * @param chan MAVLink channel to send the message - * - * @param target The system setting the mode - * @param nav_mode The new navigation mode - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target); - _mav_put_uint8_t(buf, 1, nav_mode); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_NAV_MODE, buf, 2, 10); -#else - mavlink_set_nav_mode_t packet; - packet.target = target; - packet.nav_mode = nav_mode; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_NAV_MODE, (const char *)&packet, 2, 10); -#endif -} - -#endif - -// MESSAGE SET_NAV_MODE UNPACKING - - -/** - * @brief Get field target from set_nav_mode message - * - * @return The system setting the mode - */ -static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field nav_mode from set_nav_mode message - * - * @return The new navigation mode - */ -static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a set_nav_mode message into a struct - * - * @param msg The message to decode - * @param set_nav_mode C-struct to decode the message contents into - */ -static inline void mavlink_msg_set_nav_mode_decode(const mavlink_message_t* msg, mavlink_set_nav_mode_t* set_nav_mode) -{ -#if MAVLINK_NEED_BYTE_SWAP - set_nav_mode->target = mavlink_msg_set_nav_mode_get_target(msg); - set_nav_mode->nav_mode = mavlink_msg_set_nav_mode_get_nav_mode(msg); -#else - memcpy(set_nav_mode, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h deleted file mode 100644 index fc6395099..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_system_time_utc.h +++ /dev/null @@ -1,166 +0,0 @@ -// 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; - -#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 -#define MAVLINK_MSG_ID_4_LEN 8 - - - -#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC { \ - "SYSTEM_TIME_UTC", \ - 2, \ - { { "utc_date", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_system_time_utc_t, utc_date) }, \ - { "utc_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_system_time_utc_t, utc_time) }, \ - } \ -} - - -/** - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, utc_date); - _mav_put_uint32_t(buf, 4, utc_time); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_system_time_utc_t packet; - packet.utc_date = utc_date; - packet.utc_time = utc_time; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - return mavlink_finalize_message(msg, system_id, component_id, 8, 191); -} - -/** - * @brief Pack a system_time_utc 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 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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, utc_date); - _mav_put_uint32_t(buf, 4, utc_time); - - memcpy(_MAV_PAYLOAD(msg), buf, 8); -#else - mavlink_system_time_utc_t packet; - packet.utc_date = utc_date; - packet.utc_time = utc_time; - - memcpy(_MAV_PAYLOAD(msg), &packet, 8); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 191); -} - -/** - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[8]; - _mav_put_uint32_t(buf, 0, utc_date); - _mav_put_uint32_t(buf, 4, utc_time); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, buf, 8, 191); -#else - mavlink_system_time_utc_t packet; - packet.utc_date = utc_date; - packet.utc_time = utc_time; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, (const char *)&packet, 8, 191); -#endif -} - -#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) -{ - return _MAV_RETURN_uint32_t(msg, 0); -} - -/** - * @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) -{ - return _MAV_RETURN_uint32_t(msg, 4); -} - -/** - * @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) -{ -#if MAVLINK_NEED_BYTE_SWAP - 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); -#else - memcpy(system_time_utc, _MAV_PAYLOAD(msg), 8); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h deleted file mode 100644 index 455607c44..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint.h +++ /dev/null @@ -1,430 +0,0 @@ -// MESSAGE WAYPOINT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT 39 - -typedef struct __mavlink_waypoint_t -{ - float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - float x; ///< PARAM5 / local: x position, global: latitude - float y; ///< PARAM6 / y position: global: longitude - float z; ///< PARAM7 / z position: global: altitude - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp -} mavlink_waypoint_t; - -#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 -#define MAVLINK_MSG_ID_39_LEN 36 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT { \ - "WAYPOINT", \ - 14, \ - { { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_waypoint_t, param1) }, \ - { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_waypoint_t, param2) }, \ - { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_waypoint_t, param3) }, \ - { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_waypoint_t, param4) }, \ - { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_waypoint_t, x) }, \ - { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_waypoint_t, y) }, \ - { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_waypoint_t, z) }, \ - { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_waypoint_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_waypoint_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_waypoint_t, target_component) }, \ - { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_waypoint_t, frame) }, \ - { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_waypoint_t, command) }, \ - { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_waypoint_t, current) }, \ - { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_waypoint_t, autocontinue) }, \ - } \ -} - - -/** - * @brief Pack a waypoint 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 seq Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, frame); - _mav_put_uint8_t(buf, 33, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); - - memcpy(_MAV_PAYLOAD(msg), buf, 36); -#else - mavlink_waypoint_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.x = x; - packet.y = y; - packet.z = z; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.command = command; - packet.current = current; - packet.autocontinue = autocontinue; - - memcpy(_MAV_PAYLOAD(msg), &packet, 36); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - return mavlink_finalize_message(msg, system_id, component_id, 36, 205); -} - -/** - * @brief Pack a waypoint 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 target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_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,uint16_t seq,uint8_t frame,uint8_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, frame); - _mav_put_uint8_t(buf, 33, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); - - memcpy(_MAV_PAYLOAD(msg), buf, 36); -#else - mavlink_waypoint_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.x = x; - packet.y = y; - packet.z = z; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.command = command; - packet.current = current; - packet.autocontinue = autocontinue; - - memcpy(_MAV_PAYLOAD(msg), &packet, 36); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 205); -} - -/** - * @brief Encode a waypoint 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 waypoint C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_t* waypoint) -{ - return mavlink_msg_waypoint_pack(system_id, component_id, msg, waypoint->target_system, waypoint->target_component, waypoint->seq, waypoint->frame, waypoint->command, waypoint->current, waypoint->autocontinue, waypoint->param1, waypoint->param2, waypoint->param3, waypoint->param4, waypoint->x, waypoint->y, waypoint->z); -} - -/** - * @brief Send a waypoint message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[36]; - _mav_put_float(buf, 0, param1); - _mav_put_float(buf, 4, param2); - _mav_put_float(buf, 8, param3); - _mav_put_float(buf, 12, param4); - _mav_put_float(buf, 16, x); - _mav_put_float(buf, 20, y); - _mav_put_float(buf, 24, z); - _mav_put_uint16_t(buf, 28, seq); - _mav_put_uint8_t(buf, 30, target_system); - _mav_put_uint8_t(buf, 31, target_component); - _mav_put_uint8_t(buf, 32, frame); - _mav_put_uint8_t(buf, 33, command); - _mav_put_uint8_t(buf, 34, current); - _mav_put_uint8_t(buf, 35, autocontinue); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, buf, 36, 205); -#else - mavlink_waypoint_t packet; - packet.param1 = param1; - packet.param2 = param2; - packet.param3 = param3; - packet.param4 = param4; - packet.x = x; - packet.y = y; - packet.z = z; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - packet.frame = frame; - packet.command = command; - packet.current = current; - packet.autocontinue = autocontinue; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, (const char *)&packet, 36, 205); -#endif -} - -#endif - -// MESSAGE WAYPOINT UNPACKING - - -/** - * @brief Get field target_system from waypoint message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 30); -} - -/** - * @brief Get field target_component from waypoint message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 31); -} - -/** - * @brief Get field seq from waypoint message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 28); -} - -/** - * @brief Get field frame from waypoint message - * - * @return The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - */ -static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 32); -} - -/** - * @brief Get field command from waypoint message - * - * @return The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - */ -static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 33); -} - -/** - * @brief Get field current from waypoint message - * - * @return false:0, true:1 - */ -static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 34); -} - -/** - * @brief Get field autocontinue from waypoint message - * - * @return autocontinue to next wp - */ -static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 35); -} - -/** - * @brief Get field param1 from waypoint message - * - * @return PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - */ -static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field param2 from waypoint message - * - * @return PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - */ -static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field param3 from waypoint message - * - * @return PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - */ -static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Get field param4 from waypoint message - * - * @return PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - */ -static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 12); -} - -/** - * @brief Get field x from waypoint message - * - * @return PARAM5 / local: x position, global: latitude - */ -static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 16); -} - -/** - * @brief Get field y from waypoint message - * - * @return PARAM6 / y position: global: longitude - */ -static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 20); -} - -/** - * @brief Get field z from waypoint message - * - * @return PARAM7 / z position: global: altitude - */ -static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 24); -} - -/** - * @brief Decode a waypoint message into a struct - * - * @param msg The message to decode - * @param waypoint C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_decode(const mavlink_message_t* msg, mavlink_waypoint_t* waypoint) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint->param1 = mavlink_msg_waypoint_get_param1(msg); - waypoint->param2 = mavlink_msg_waypoint_get_param2(msg); - waypoint->param3 = mavlink_msg_waypoint_get_param3(msg); - waypoint->param4 = mavlink_msg_waypoint_get_param4(msg); - waypoint->x = mavlink_msg_waypoint_get_x(msg); - waypoint->y = mavlink_msg_waypoint_get_y(msg); - waypoint->z = mavlink_msg_waypoint_get_z(msg); - waypoint->seq = mavlink_msg_waypoint_get_seq(msg); - waypoint->target_system = mavlink_msg_waypoint_get_target_system(msg); - waypoint->target_component = mavlink_msg_waypoint_get_target_component(msg); - waypoint->frame = mavlink_msg_waypoint_get_frame(msg); - waypoint->command = mavlink_msg_waypoint_get_command(msg); - waypoint->current = mavlink_msg_waypoint_get_current(msg); - waypoint->autocontinue = mavlink_msg_waypoint_get_autocontinue(msg); -#else - memcpy(waypoint, _MAV_PAYLOAD(msg), 36); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h deleted file mode 100644 index 846ae8839..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_ack.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_ACK PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_ACK 47 - -typedef struct __mavlink_waypoint_ack_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t type; ///< 0: OK, 1: Error -} mavlink_waypoint_ack_t; - -#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 -#define MAVLINK_MSG_ID_47_LEN 3 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_ACK { \ - "WAYPOINT_ACK", \ - 3, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_ack_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_ack_t, target_component) }, \ - { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_ack_t, type) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_ack 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 type 0: OK, 1: Error - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_waypoint_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - return mavlink_finalize_message(msg, system_id, component_id, 3, 214); -} - -/** - * @brief Pack a waypoint_ack 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 target_system System ID - * @param target_component Component ID - * @param type 0: OK, 1: Error - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_ack_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 type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - memcpy(_MAV_PAYLOAD(msg), buf, 3); -#else - mavlink_waypoint_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - memcpy(_MAV_PAYLOAD(msg), &packet, 3); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 214); -} - -/** - * @brief Encode a waypoint_ack 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 waypoint_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_ack_t* waypoint_ack) -{ - return mavlink_msg_waypoint_ack_pack(system_id, component_id, msg, waypoint_ack->target_system, waypoint_ack->target_component, waypoint_ack->type); -} - -/** - * @brief Send a waypoint_ack message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param type 0: OK, 1: Error - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[3]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - _mav_put_uint8_t(buf, 2, type); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_ACK, buf, 3, 214); -#else - mavlink_waypoint_ack_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - packet.type = type; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_ACK, (const char *)&packet, 3, 214); -#endif -} - -#endif - -// MESSAGE WAYPOINT_ACK UNPACKING - - -/** - * @brief Get field target_system from waypoint_ack message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from waypoint_ack message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field type from waypoint_ack message - * - * @return 0: OK, 1: Error - */ -static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Decode a waypoint_ack message into a struct - * - * @param msg The message to decode - * @param waypoint_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_ack_decode(const mavlink_message_t* msg, mavlink_waypoint_ack_t* waypoint_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_ack->target_system = mavlink_msg_waypoint_ack_get_target_system(msg); - waypoint_ack->target_component = mavlink_msg_waypoint_ack_get_target_component(msg); - waypoint_ack->type = mavlink_msg_waypoint_ack_get_type(msg); -#else - memcpy(waypoint_ack, _MAV_PAYLOAD(msg), 3); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h deleted file mode 100644 index 4133b58d0..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_clear_all.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE WAYPOINT_CLEAR_ALL PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 - -typedef struct __mavlink_waypoint_clear_all_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_clear_all_t; - -#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 -#define MAVLINK_MSG_ID_45_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL { \ - "WAYPOINT_CLEAR_ALL", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_clear_all_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_clear_all_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_clear_all 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - return mavlink_finalize_message(msg, system_id, component_id, 2, 229); -} - -/** - * @brief Pack a waypoint_clear_all 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 target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_clear_all_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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 229); -} - -/** - * @brief Encode a waypoint_clear_all 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 waypoint_clear_all C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_clear_all_t* waypoint_clear_all) -{ - return mavlink_msg_waypoint_clear_all_pack(system_id, component_id, msg, waypoint_clear_all->target_system, waypoint_clear_all->target_component); -} - -/** - * @brief Send a waypoint_clear_all message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, buf, 2, 229); -#else - mavlink_waypoint_clear_all_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, (const char *)&packet, 2, 229); -#endif -} - -#endif - -// MESSAGE WAYPOINT_CLEAR_ALL UNPACKING - - -/** - * @brief Get field target_system from waypoint_clear_all message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from waypoint_clear_all message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a waypoint_clear_all message into a struct - * - * @param msg The message to decode - * @param waypoint_clear_all C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_clear_all_decode(const mavlink_message_t* msg, mavlink_waypoint_clear_all_t* waypoint_clear_all) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_clear_all->target_system = mavlink_msg_waypoint_clear_all_get_target_system(msg); - waypoint_clear_all->target_component = mavlink_msg_waypoint_clear_all_get_target_component(msg); -#else - memcpy(waypoint_clear_all, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h deleted file mode 100644 index cfa433338..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_count.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_COUNT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 - -typedef struct __mavlink_waypoint_count_t -{ - uint16_t count; ///< Number of Waypoints in the Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_count_t; - -#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 -#define MAVLINK_MSG_ID_44_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT { \ - "WAYPOINT_COUNT", \ - 3, \ - { { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_count_t, count) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_count_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_count_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_count 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 count Number of Waypoints in the Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - return mavlink_finalize_message(msg, system_id, component_id, 4, 8); -} - -/** - * @brief Pack a waypoint_count 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 target_system System ID - * @param target_component Component ID - * @param count Number of Waypoints in the Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_count_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,uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 8); -} - -/** - * @brief Encode a waypoint_count 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 waypoint_count C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_count_t* waypoint_count) -{ - return mavlink_msg_waypoint_count_pack(system_id, component_id, msg, waypoint_count->target_system, waypoint_count->target_component, waypoint_count->count); -} - -/** - * @brief Send a waypoint_count message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param count Number of Waypoints in the Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, count); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, buf, 4, 8); -#else - mavlink_waypoint_count_t packet; - packet.count = count; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, (const char *)&packet, 4, 8); -#endif -} - -#endif - -// MESSAGE WAYPOINT_COUNT UNPACKING - - -/** - * @brief Get field target_system from waypoint_count message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from waypoint_count message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field count from waypoint_count message - * - * @return Number of Waypoints in the Sequence - */ -static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_count message into a struct - * - * @param msg The message to decode - * @param waypoint_count C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_count_decode(const mavlink_message_t* msg, mavlink_waypoint_count_t* waypoint_count) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_count->count = mavlink_msg_waypoint_count_get_count(msg); - waypoint_count->target_system = mavlink_msg_waypoint_count_get_target_system(msg); - waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); -#else - memcpy(waypoint_count, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h deleted file mode 100644 index a2438a456..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_current.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE WAYPOINT_CURRENT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 - -typedef struct __mavlink_waypoint_current_t -{ - uint16_t seq; ///< Sequence -} mavlink_waypoint_current_t; - -#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 -#define MAVLINK_MSG_ID_42_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT { \ - "WAYPOINT_CURRENT", \ - 1, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_current_t, seq) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_current 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_current_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - return mavlink_finalize_message(msg, system_id, component_id, 2, 101); -} - -/** - * @brief Pack a waypoint_current 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_current_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 101); -} - -/** - * @brief Encode a waypoint_current 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 waypoint_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_current_t* waypoint_current) -{ - return mavlink_msg_waypoint_current_pack(system_id, component_id, msg, waypoint_current->seq); -} - -/** - * @brief Send a waypoint_current message - * @param chan MAVLink channel to send the message - * - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, buf, 2, 101); -#else - mavlink_waypoint_current_t packet; - packet.seq = seq; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, (const char *)&packet, 2, 101); -#endif -} - -#endif - -// MESSAGE WAYPOINT_CURRENT UNPACKING - - -/** - * @brief Get field seq from waypoint_current message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_current message into a struct - * - * @param msg The message to decode - * @param waypoint_current C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_current_decode(const mavlink_message_t* msg, mavlink_waypoint_current_t* waypoint_current) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); -#else - memcpy(waypoint_current, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h deleted file mode 100644 index e1d64b4b6..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_reached.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE WAYPOINT_REACHED PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 - -typedef struct __mavlink_waypoint_reached_t -{ - uint16_t seq; ///< Sequence -} mavlink_waypoint_reached_t; - -#define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 -#define MAVLINK_MSG_ID_46_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED { \ - "WAYPOINT_REACHED", \ - 1, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_reached_t, seq) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_reached 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_reached_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - return mavlink_finalize_message(msg, system_id, component_id, 2, 21); -} - -/** - * @brief Pack a waypoint_reached 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_reached_t packet; - packet.seq = seq; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 21); -} - -/** - * @brief Encode a waypoint_reached 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 waypoint_reached C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_reached_t* waypoint_reached) -{ - return mavlink_msg_waypoint_reached_pack(system_id, component_id, msg, waypoint_reached->seq); -} - -/** - * @brief Send a waypoint_reached message - * @param chan MAVLink channel to send the message - * - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint16_t(buf, 0, seq); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, buf, 2, 21); -#else - mavlink_waypoint_reached_t packet; - packet.seq = seq; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, (const char *)&packet, 2, 21); -#endif -} - -#endif - -// MESSAGE WAYPOINT_REACHED UNPACKING - - -/** - * @brief Get field seq from waypoint_reached message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_reached message into a struct - * - * @param msg The message to decode - * @param waypoint_reached C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_reached_decode(const mavlink_message_t* msg, mavlink_waypoint_reached_t* waypoint_reached) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); -#else - memcpy(waypoint_reached, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h deleted file mode 100644 index b88f4dbfb..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_REQUEST PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 - -typedef struct __mavlink_waypoint_request_t -{ - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_request_t; - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 -#define MAVLINK_MSG_ID_40_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST { \ - "WAYPOINT_REQUEST", \ - 3, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_request_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_request_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_request_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_request 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - return mavlink_finalize_message(msg, system_id, component_id, 4, 51); -} - -/** - * @brief Pack a waypoint_request 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 target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_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,uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 51); -} - -/** - * @brief Encode a waypoint_request 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 waypoint_request C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_request_t* waypoint_request) -{ - return mavlink_msg_waypoint_request_pack(system_id, component_id, msg, waypoint_request->target_system, waypoint_request->target_component, waypoint_request->seq); -} - -/** - * @brief Send a waypoint_request message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, buf, 4, 51); -#else - mavlink_waypoint_request_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, (const char *)&packet, 4, 51); -#endif -} - -#endif - -// MESSAGE WAYPOINT_REQUEST UNPACKING - - -/** - * @brief Get field target_system from waypoint_request message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from waypoint_request message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field seq from waypoint_request message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_request message into a struct - * - * @param msg The message to decode - * @param waypoint_request C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_request_decode(const mavlink_message_t* msg, mavlink_waypoint_request_t* waypoint_request) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_request->seq = mavlink_msg_waypoint_request_get_seq(msg); - waypoint_request->target_system = mavlink_msg_waypoint_request_get_target_system(msg); - waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); -#else - memcpy(waypoint_request, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h deleted file mode 100644 index 61e8d834b..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_request_list.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE WAYPOINT_REQUEST_LIST PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 - -typedef struct __mavlink_waypoint_request_list_t -{ - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_request_list_t; - -#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 -#define MAVLINK_MSG_ID_43_LEN 2 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST { \ - "WAYPOINT_REQUEST_LIST", \ - 2, \ - { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_request_list_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_request_list_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_request_list 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 - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - return mavlink_finalize_message(msg, system_id, component_id, 2, 213); -} - -/** - * @brief Pack a waypoint_request_list 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 target_system System ID - * @param target_component Component ID - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_request_list_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) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 2); -#else - mavlink_waypoint_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 2); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 213); -} - -/** - * @brief Encode a waypoint_request_list 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 waypoint_request_list C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_request_list_t* waypoint_request_list) -{ - return mavlink_msg_waypoint_request_list_pack(system_id, component_id, msg, waypoint_request_list->target_system, waypoint_request_list->target_component); -} - -/** - * @brief Send a waypoint_request_list message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[2]; - _mav_put_uint8_t(buf, 0, target_system); - _mav_put_uint8_t(buf, 1, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, buf, 2, 213); -#else - mavlink_waypoint_request_list_t packet; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, (const char *)&packet, 2, 213); -#endif -} - -#endif - -// MESSAGE WAYPOINT_REQUEST_LIST UNPACKING - - -/** - * @brief Get field target_system from waypoint_request_list message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field target_component from waypoint_request_list message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Decode a waypoint_request_list message into a struct - * - * @param msg The message to decode - * @param waypoint_request_list C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_request_list_decode(const mavlink_message_t* msg, mavlink_waypoint_request_list_t* waypoint_request_list) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_request_list->target_system = mavlink_msg_waypoint_request_list_get_target_system(msg); - waypoint_request_list->target_component = mavlink_msg_waypoint_request_list_get_target_component(msg); -#else - memcpy(waypoint_request_list, _MAV_PAYLOAD(msg), 2); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h b/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h deleted file mode 100644 index e2c683ee3..000000000 --- a/src/GCS_MAVLink/include_v1.0/common/mavlink_msg_waypoint_set_current.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE WAYPOINT_SET_CURRENT PACKING - -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 - -typedef struct __mavlink_waypoint_set_current_t -{ - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID -} mavlink_waypoint_set_current_t; - -#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 -#define MAVLINK_MSG_ID_41_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT { \ - "WAYPOINT_SET_CURRENT", \ - 3, \ - { { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_set_current_t, seq) }, \ - { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_set_current_t, target_system) }, \ - { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_waypoint_set_current_t, target_component) }, \ - } \ -} - - -/** - * @brief Pack a waypoint_set_current 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 seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - return mavlink_finalize_message(msg, system_id, component_id, 4, 80); -} - -/** - * @brief Pack a waypoint_set_current 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 target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_waypoint_set_current_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,uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - memcpy(_MAV_PAYLOAD(msg), buf, 4); -#else - mavlink_waypoint_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - memcpy(_MAV_PAYLOAD(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 80); -} - -/** - * @brief Encode a waypoint_set_current 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 waypoint_set_current C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_waypoint_set_current_t* waypoint_set_current) -{ - return mavlink_msg_waypoint_set_current_pack(system_id, component_id, msg, waypoint_set_current->target_system, waypoint_set_current->target_component, waypoint_set_current->seq); -} - -/** - * @brief Send a waypoint_set_current message - * @param chan MAVLink channel to send the message - * - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint16_t(buf, 0, seq); - _mav_put_uint8_t(buf, 2, target_system); - _mav_put_uint8_t(buf, 3, target_component); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, buf, 4, 80); -#else - mavlink_waypoint_set_current_t packet; - packet.seq = seq; - packet.target_system = target_system; - packet.target_component = target_component; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, (const char *)&packet, 4, 80); -#endif -} - -#endif - -// MESSAGE WAYPOINT_SET_CURRENT UNPACKING - - -/** - * @brief Get field target_system from waypoint_set_current message - * - * @return System ID - */ -static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field target_component from waypoint_set_current message - * - * @return Component ID - */ -static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Get field seq from waypoint_set_current message - * - * @return Sequence - */ -static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint16_t(msg, 0); -} - -/** - * @brief Decode a waypoint_set_current message into a struct - * - * @param msg The message to decode - * @param waypoint_set_current C-struct to decode the message contents into - */ -static inline void mavlink_msg_waypoint_set_current_decode(const mavlink_message_t* msg, mavlink_waypoint_set_current_t* waypoint_set_current) -{ -#if MAVLINK_NEED_BYTE_SWAP - waypoint_set_current->seq = mavlink_msg_waypoint_set_current_get_seq(msg); - waypoint_set_current->target_system = mavlink_msg_waypoint_set_current_get_target_system(msg); - waypoint_set_current->target_component = mavlink_msg_waypoint_set_current_get_target_component(msg); -#else - memcpy(waypoint_set_current, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/src/GCS_MAVLink/include_v1.0/mavlink_conversions.h b/src/GCS_MAVLink/include_v1.0/mavlink_conversions.h new file mode 100644 index 000000000..d89363577 --- /dev/null +++ b/src/GCS_MAVLink/include_v1.0/mavlink_conversions.h @@ -0,0 +1,131 @@ +#ifndef _MAVLINK_CONVERSIONS_H_ +#define _MAVLINK_CONVERSIONS_H_ + +/* enable math defines on Windows */ +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif +#include + +/** + * @file mavlink_conversions.h + * + * These conversion functions follow the NASA rotation standards definition file + * available online. + * + * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free + * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free) + * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the + * protocol as widely as possible. + * + * @author James Goppert + */ + +MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3]) +{ + double a = quaternion[0]; + double b = quaternion[1]; + double c = quaternion[2]; + double d = quaternion[3]; + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; + dcm[0][0] = aSq + bSq - cSq - dSq; + dcm[0][1] = 2.0 * (b * c - a * d); + dcm[0][2] = 2.0 * (a * c + b * d); + dcm[1][0] = 2.0 * (b * c + a * d); + dcm[1][1] = aSq - bSq + cSq - dSq; + dcm[1][2] = 2.0 * (c * d - a * b); + dcm[2][0] = 2.0 * (b * d - a * c); + dcm[2][1] = 2.0 * (a * b + c * d); + dcm[2][2] = aSq - bSq - cSq + dSq; +} + +MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw) +{ + float phi, theta, psi; + theta = asin(-dcm[2][0]); + + if (fabs(theta - M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = (atan2(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1]) + phi); + + } else if (fabs(theta + M_PI_2) < 1.0e-3f) { + phi = 0.0f; + psi = atan2f(dcm[1][2] - dcm[0][1], + dcm[0][2] + dcm[1][1] - phi); + + } else { + phi = atan2f(dcm[2][1], dcm[2][2]); + psi = atan2f(dcm[1][0], dcm[0][0]); + } + + *roll = phi; + *pitch = theta; + *yaw = psi; +} + +MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw) +{ + float dcm[3][3]; + mavlink_quaternion_to_dcm(quaternion, dcm); + mavlink_dcm_to_euler(dcm, roll, pitch, yaw); +} + +MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4]) +{ + double cosPhi_2 = cos((double)roll / 2.0); + double sinPhi_2 = sin((double)roll / 2.0); + double cosTheta_2 = cos((double)pitch / 2.0); + double sinTheta_2 = sin((double)pitch / 2.0); + double cosPsi_2 = cos((double)yaw / 2.0); + double sinPsi_2 = sin((double)yaw / 2.0); + quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 + + sinPhi_2 * sinTheta_2 * sinPsi_2); + quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 - + cosPhi_2 * sinTheta_2 * sinPsi_2); + quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 + + sinPhi_2 * cosTheta_2 * sinPsi_2); + quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 - + sinPhi_2 * sinTheta_2 * cosPsi_2); +} + +MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4]) +{ + quaternion[0] = (0.5 * sqrt(1.0 + + (double)(dcm[0][0] + dcm[1][1] + dcm[2][2]))); + quaternion[1] = (0.5 * sqrt(1.0 + + (double)(dcm[0][0] - dcm[1][1] - dcm[2][2]))); + quaternion[2] = (0.5 * sqrt(1.0 + + (double)(-dcm[0][0] + dcm[1][1] - dcm[2][2]))); + quaternion[3] = (0.5 * sqrt(1.0 + + (double)(-dcm[0][0] - dcm[1][1] + dcm[2][2]))); +} + +MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3]) +{ + double cosPhi = cos(roll); + double sinPhi = sin(roll); + double cosThe = cos(pitch); + double sinThe = sin(pitch); + double cosPsi = cos(yaw); + double sinPsi = sin(yaw); + + dcm[0][0] = cosThe * cosPsi; + dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; + dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; + + dcm[1][0] = cosThe * sinPsi; + dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; + dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; + + dcm[2][0] = -sinThe; + dcm[2][1] = sinPhi * cosThe; + dcm[2][2] = cosPhi * cosThe; +} + +#endif \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/mavlink_missionlib_data.h b/src/GCS_MAVLink/missionlib/mavlink_missionlib_data.h deleted file mode 100644 index c825fbcef..000000000 --- a/src/GCS_MAVLink/missionlib/mavlink_missionlib_data.h +++ /dev/null @@ -1,58 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -// Disable auto-data structures -#ifndef MAVLINK_NO_DATA -#define MAVLINK_NO_DATA -#endif - -#include "mavlink.h" -#include - -/* MISSION LIB DATA STORAGE */ - -enum MAVLINK_PM_PARAMETERS -{ - MAVLINK_PM_PARAM_SYSTEM_ID, - MAVLINK_PM_PARAM_ATT_K_D, - MAVLINK_PM_MAX_PARAM_COUNT // VERY IMPORTANT! KEEP THIS AS LAST ENTRY -}; - - -/* DO NOT EDIT THIS FILE BELOW THIS POINT! */ - -#ifndef MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN -#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 -#endif - - -//extern void mavlink_pm_queued_send(); -struct mavlink_pm_storage { - char param_names[MAVLINK_PM_MAX_PARAM_COUNT][MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; ///< Parameter names - float param_values[MAVLINK_PM_MAX_PARAM_COUNT]; ///< Parameter values - uint16_t next_param; - uint16_t size; -}; - -typedef struct mavlink_pm_storage mavlink_pm_storage; - -void mavlink_pm_reset_params(mavlink_pm_storage* pm); diff --git a/src/GCS_MAVLink/missionlib/mavlink_parameters.c b/src/GCS_MAVLink/missionlib/mavlink_parameters.c deleted file mode 100644 index 18a05b3ec..000000000 --- a/src/GCS_MAVLink/missionlib/mavlink_parameters.c +++ /dev/null @@ -1,150 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ - -#include "mavlink_missionlib_data.h" -#include "mavlink_parameters.h" -#include "math.h" /* isinf / isnan checks */ - -extern mavlink_system_t mavlink_system; -extern mavlink_pm_storage pm; - -extern void mavlink_missionlib_send_message(mavlink_message_t* msg); -extern void mavlink_missionlib_send_gcs_string(const char* string); - -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg) -{ - switch (msg->msgid) - { - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - { - // Start sending parameters - pm.next_param = 0; - mavlink_missionlib_send_gcs_string("PM SENDING LIST"); - } - break; - case MAVLINK_MSG_ID_PARAM_SET: - { - mavlink_param_set_t set; - mavlink_msg_param_set_decode(msg, &set); - - // Check if this message is for this system - if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid) - { - char* key = set.param_id; - - for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++) - { - bool match = true; - for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++) - { - // Compare - if (pm.param_names[i][j] != key[j]) - { - match = false; - } - - // End matching if null termination is reached - if (pm.param_names[i][j] == '\0') - { - break; - } - } - - // Check if matched - if (match) - { - // Only write and emit changes if there is actually a difference - // AND only write if new value is NOT "not-a-number" - // AND is NOT infinity - if (pm.param_values[i] != set.param_value - && !isnan(set.param_value) - && !isinf(set.param_value)) - { - pm.param_values[i] = set.param_value; - // Report back new value -#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS - mavlink_message_t tx_msg; - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - MAVLINK_COMM_0, - &tx_msg, - pm.param_names[i], - pm.param_values[i], - MAVLINK_TYPE_FLOAT, - pm.size, - i); - mavlink_missionlib_send_message(&tx_msg); -#else - mavlink_msg_param_value_send(MAVLINK_COMM_0, - pm.param_names[i], - pm.param_values[i], - MAVLINK_TYPE_FLOAT, - pm.size, - i); -#endif - - mavlink_missionlib_send_gcs_string("PM received param"); - } // End valid value check - } // End match check - } // End for loop - } // End system ID check - - } // End case - break; - - } // End switch - -} - - /** - * @brief Send low-priority messages at a maximum rate of xx Hertz - * - * This function sends messages at a lower rate to not exceed the wireless - * bandwidth. It sends one message each time it is called until the buffer is empty. - * Call this function with xx Hertz to increase/decrease the bandwidth. - */ - void mavlink_pm_queued_send(void) - { - //send parameters one by one - if (pm.next_param < pm.size) - { - //for (int i.. all active comm links) -#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS - mavlink_message_t tx_msg; - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, - mavlink_system.compid, - MAVLINK_COMM_0, - &tx_msg, - pm.param_names[pm.next_param], - pm.param_values[pm.next_param], - MAVLINK_TYPE_FLOAT, - pm.size, - pm.next_param); - mavlink_missionlib_send_message(&tx_msg); -#else - mavlink_msg_param_value_send(MAVLINK_COMM_0, - pm.param_names[pm.next_param], - pm.param_values[pm.next_param], - MAV_DATA_TYPE_FLOAT, - pm.size, - pm.next_param); -#endif - pm.next_param++; - } - } diff --git a/src/GCS_MAVLink/missionlib/mavlink_parameters.h b/src/GCS_MAVLink/missionlib/mavlink_parameters.h deleted file mode 100644 index 0d7841fcd..000000000 --- a/src/GCS_MAVLink/missionlib/mavlink_parameters.h +++ /dev/null @@ -1,38 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -// Disable auto-data structures -#ifndef MAVLINK_NO_DATA -#define MAVLINK_NO_DATA -#endif - -#include "mavlink.h" -#include - -/* PARAMETER MANAGER - MISSION LIB */ - -#ifndef MAVLINK_PM_TEXT_FEEDBACK -#define MAVLINK_PM_TEXT_FEEDBACK 1 ///< Report back status information as text -#endif - -void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg); -void mavlink_pm_queued_send(void); \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/testing/.gitignore b/src/GCS_MAVLink/missionlib/testing/.gitignore deleted file mode 100644 index 0776965a6..000000000 --- a/src/GCS_MAVLink/missionlib/testing/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -*.o -udptest -build diff --git a/src/GCS_MAVLink/missionlib/testing/Makefile b/src/GCS_MAVLink/missionlib/testing/Makefile deleted file mode 100644 index 134aa47fc..000000000 --- a/src/GCS_MAVLink/missionlib/testing/Makefile +++ /dev/null @@ -1,12 +0,0 @@ -VERSION = 1.00 -CC = gcc -CFLAGS = -I ../../include/common -Wall -Werror -DVERSION=\"$(MAVLINK_VERSION)\" -std=c99 -LDFLAGS = "" - -OBJ = udp.o - -udptest: $(OBJ) - $(CC) $(CFLAGS) -o udptest $(OBJ) $(LDFLAGS) - -%.o: %.c - $(CC) $(CFLAGS) -c $< \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/testing/main.c b/src/GCS_MAVLink/missionlib/testing/main.c deleted file mode 100644 index 21aa1f7e1..000000000 --- a/src/GCS_MAVLink/missionlib/testing/main.c +++ /dev/null @@ -1,373 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - and Bryan Godbolt godbolt ( a t ) ualberta.ca - - adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ -/* - This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets - cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from - qgroundcontrol are printed by this program along with the heartbeats. - - - I compiled this program sucessfully on Ubuntu 10.04 with the following command - - gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c - - the rt library is needed for the clock_gettime on linux - */ -/* These headers are for QNX, but should all be standard on unix/linux */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if (defined __QNX__) | (defined __QNXNTO__) -/* QNX specific headers */ -#include -#else -/* Linux / MacOS POSIX timer headers */ -#include -#include -#include -#endif - -/* FIRST: MAVLink setup */ -//#define MAVLINK_CONFIGURED -//#define MAVLINK_NO_DATA -//#define MAVLINK_WPM_VERBOSE - -/* 0: Include MAVLink types */ -#include "mavlink_types.h" - -/* 1: Define mavlink system storage */ -mavlink_system_t mavlink_system; - -/* 2: Include actual protocol, REQUIRES mavlink_system */ -#include "mavlink.h" - -/* 3: Define waypoint helper functions */ -void mavlink_wpm_send_message(mavlink_message_t* msg); -void mavlink_wpm_send_gcs_string(const char* string); -uint64_t mavlink_wpm_get_system_timestamp(); -void mavlink_missionlib_send_message(mavlink_message_t* msg); -void mavlink_missionlib_send_gcs_string(const char* string); -uint64_t mavlink_missionlib_get_system_timestamp(); - -/* 4: Include waypoint protocol */ -#include "waypoints.h" -mavlink_wpm_storage wpm; - - -#include "mavlink_missionlib_data.h" -#include "mavlink_parameters.h" - -mavlink_pm_storage pm; - -/** - * @brief reset all parameters to default - * @warning DO NOT USE THIS IN FLIGHT! - */ -void mavlink_pm_reset_params(mavlink_pm_storage* pm) -{ - pm->size = MAVLINK_PM_MAX_PARAM_COUNT; - // 1) MAVLINK_PM_PARAM_SYSTEM_ID - pm->param_values[MAVLINK_PM_PARAM_SYSTEM_ID] = 12; - strcpy(pm->param_names[MAVLINK_PM_PARAM_SYSTEM_ID], "SYS_ID"); - // 2) MAVLINK_PM_PARAM_ATT_K_D - pm->param_values[MAVLINK_PM_PARAM_ATT_K_D] = 0.3f; - strcpy(pm->param_names[MAVLINK_PM_PARAM_ATT_K_D], "ATT_K_D"); -} - - -#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) - -char help[] = "--help"; - - -char target_ip[100]; - -float position[6] = {}; -int sock; -struct sockaddr_in gcAddr; -struct sockaddr_in locAddr; -uint8_t buf[BUFFER_LENGTH]; -ssize_t recsize; -socklen_t fromlen; -int bytes_sent; -mavlink_message_t msg; -uint16_t len; -int i = 0; -unsigned int temp = 0; - -uint64_t microsSinceEpoch(); - - - - -/* Provide the interface functions for the waypoint manager */ - -/* - * @brief Sends a MAVLink message over UDP - */ - -void mavlink_missionlib_send_message(mavlink_message_t* msg) -{ - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - printf("SENT %d bytes\n", bytes_sent); -} - -void mavlink_missionlib_send_gcs_string(const char* string) -{ - const int len = 50; - mavlink_statustext_t status; - int i = 0; - while (i < len - 1) - { - status.text[i] = string[i]; - if (string[i] == '\0') - break; - i++; - } - status.text[i] = '\0'; // Enforce null termination - mavlink_message_t msg; - - mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &status); - mavlink_missionlib_send_message(&msg); -} - -uint64_t mavlink_missionlib_get_system_timestamp() -{ - struct timeval tv; - gettimeofday(&tv, NULL); - return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; -} - -float roll, pitch, yaw; -uint32_t latitude, longitude, altitude; -uint16_t speedx, speedy, speedz; -float rollspeed, pitchspeed, yawspeed; -bool hilEnabled = false; - -int main(int argc, char* argv[]) -{ - // Initialize MAVLink - mavlink_wpm_init(&wpm); - mavlink_system.sysid = 5; - mavlink_system.compid = 20; - mavlink_pm_reset_params(&pm); - - int32_t ground_distance; - uint32_t time_ms; - - - - // Create socket - sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - - // Check if --help flag was used - if ((argc == 2) && (strcmp(argv[1], help) == 0)) - { - printf("\n"); - printf("\tUsage:\n\n"); - printf("\t"); - printf("%s", argv[0]); - printf(" \n"); - printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); - exit(EXIT_FAILURE); - } - - - // Change the target ip if parameter was given - strcpy(target_ip, "127.0.0.1"); - if (argc == 2) - { - strcpy(target_ip, argv[1]); - } - - - memset(&locAddr, 0, sizeof(locAddr)); - locAddr.sin_family = AF_INET; - locAddr.sin_addr.s_addr = INADDR_ANY; - locAddr.sin_port = htons(14551); - - /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ - if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) - { - perror("error bind failed"); - close(sock); - exit(EXIT_FAILURE); - } - - /* Attempt to make it non blocking */ - if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) - { - fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); - close(sock); - exit(EXIT_FAILURE); - } - - - memset(&gcAddr, 0, sizeof(gcAddr)); - gcAddr.sin_family = AF_INET; - gcAddr.sin_addr.s_addr = inet_addr(target_ip); - gcAddr.sin_port = htons(14550); - - - printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); - - - for (;;) - { - bytes_sent = 0; - - /* Send Heartbeat */ - mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send Status */ - mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - /* Send Local Position */ - mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), - position[0], position[1], position[2], - position[3], position[4], position[5]); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send global position */ - if (hilEnabled) - { - mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - } - - /* Send attitude */ - mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send HIL outputs */ - float roll_ailerons = 0; // -1 .. 1 - float pitch_elevator = 0.2; // -1 .. 1 - float yaw_rudder = 0.1; // -1 .. 1 - float throttle = 0.9; // 0 .. 1 - mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - memset(buf, 0, BUFFER_LENGTH); - recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); - if (recsize > 0) - { - // Something received - print out all bytes and parse packet - mavlink_message_t msg; - mavlink_status_t status; - - printf("Bytes Received: %d\nDatagram: ", (int)recsize); - for (i = 0; i < recsize; ++i) - { - temp = buf[i]; - printf("%02x ", (unsigned char)temp); - if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) - { - // Packet received - printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); - - // Handle packet with waypoint component - mavlink_wpm_message_handler(&msg); - - // Handle packet with parameter component - mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); - - // Print HIL values sent to system - if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE) - { - mavlink_hil_state_t hil; - mavlink_msg_hil_state_decode(&msg, &hil); - printf("Received HIL state:\n"); - printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw); - roll = hil.roll; - pitch = hil.pitch; - yaw = hil.yaw; - rollspeed = hil.rollspeed; - pitchspeed = hil.pitchspeed; - yawspeed = hil.yawspeed; - speedx = hil.vx; - speedy = hil.vy; - speedz = hil.vz; - latitude = hil.lat; - longitude = hil.lon; - altitude = hil.alt; - hilEnabled = true; - } - } - } - printf("\n"); - } - memset(buf, 0, BUFFER_LENGTH); - usleep(10000); // Sleep 10 ms - - - // Send one parameter - mavlink_pm_queued_send(); - } -} - - -/* QNX timer version */ -#if (defined __QNX__) | (defined __QNXNTO__) -uint64_t microsSinceEpoch() -{ - - struct timespec time; - - uint64_t micros = 0; - - clock_gettime(CLOCK_REALTIME, &time); - micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; - - return micros; -} -#else -uint64_t microsSinceEpoch() -{ - - struct timeval tv; - - uint64_t micros = 0; - - gettimeofday(&tv, NULL); - micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - - return micros; -} -#endif \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/testing/mavlink_missionlib_data.h b/src/GCS_MAVLink/missionlib/testing/mavlink_missionlib_data.h deleted file mode 100644 index a040941e2..000000000 --- a/src/GCS_MAVLink/missionlib/testing/mavlink_missionlib_data.h +++ /dev/null @@ -1,54 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -// Disable auto-data structures -#ifndef MAVLINK_NO_DATA -#define MAVLINK_NO_DATA -#endif - -#include "mavlink.h" -#include - -/* MISSION LIB DATA STORAGE */ - -enum MAVLINK_PM_PARAMETERS -{ - MAVLINK_PM_PARAM_SYSTEM_ID, - MAVLINK_PM_PARAM_ATT_K_D, - MAVLINK_PM_MAX_PARAM_COUNT // VERY IMPORTANT! KEEP THIS AS LAST ENTRY -}; - - -/* DO NOT EDIT THIS FILE BELOW THIS POINT! */ - - -//extern void mavlink_pm_queued_send(); -struct mavlink_pm_storage { - char param_names[MAVLINK_PM_MAX_PARAM_COUNT][MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; ///< Parameter names - float param_values[MAVLINK_PM_MAX_PARAM_COUNT]; ///< Parameter values - uint16_t next_param; - uint16_t size; -}; - -typedef struct mavlink_pm_storage mavlink_pm_storage; - -void mavlink_pm_reset_params(mavlink_pm_storage* pm); \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/testing/udp.c b/src/GCS_MAVLink/missionlib/testing/udp.c deleted file mode 100644 index dc096be51..000000000 --- a/src/GCS_MAVLink/missionlib/testing/udp.c +++ /dev/null @@ -1,1073 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - and Bryan Godbolt godbolt ( a t ) ualberta.ca - - adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ -/* - This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets - cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from - qgroundcontrol are printed by this program along with the heartbeats. - - - I compiled this program sucessfully on Ubuntu 10.04 with the following command - - gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c - - the rt library is needed for the clock_gettime on linux - */ -/* These headers are for QNX, but should all be standard on unix/linux */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if (defined __QNX__) | (defined __QNXNTO__) -/* QNX specific headers */ -#include -#else -/* Linux / MacOS POSIX timer headers */ -#include -#include -#include -#endif - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ -#include - - -#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) - -uint64_t microsSinceEpoch(); - - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES -{ - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES -{ - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 100 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#define MAVLINK_WPM_TEXT_FEEDBACK ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 - -struct _mavlink_wpm_storage { - mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t count; - MAVLINK_WPM_STATES current_state; -} mavlink_wpm_storage; - - -void mavlink_wpm_init(mavlink_wpm_storage* state) -{ - // Set all waypoints to zero - - // Set count to zero - state->count = 0; - state->current_state = MAVLINK_WPM_STATE_IDLE; -} - - -PX_WAYPOINTPLANNER_STATES current_state = PX_WPP_IDLE; -uint16_t protocol_current_wp_id = 0; -uint16_t protocol_current_count = 0; -uint8_t protocol_current_partner_systemid = 0; -uint8_t protocol_current_partner_compid = 0; -uint64_t protocol_timestamp_lastaction = 0; - -uint64_t timestamp_last_send_setpoint = 0; - - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_waypoint_ack_t wpa; - - wpa.target_system = target_systemid; - wpa.target_component = target_compid; - wpa.type = type; - - mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if(seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_waypoint_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if(seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_local_position_setpoint_set_t PControlSetPoint; - - // send new set point to local IMU - if (cur->frame == 1) - { - PControlSetPoint.target_system = systemid; - PControlSetPoint.target_component = MAV_COMP_ID_IMU; - PControlSetPoint.x = cur->x; - PControlSetPoint.y = cur->y; - PControlSetPoint.z = cur->z; - PControlSetPoint.yaw = cur->param4; - - mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); - } - - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - timestamp_last_send_setpoint = now; - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_waypoint_count_t wpc; - - wpc.target_system = target_systemid; - wpc.target_component = target_compid; - wpc.count = count; - - mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) - { - mavlink_message_t msg; - mavlink_waypoint_t *wp = waypoints->at(seq); - wp->target_system = target_systemid; - wp->target_component = target_compid; - mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) - { - mavlink_message_t msg; - mavlink_waypoint_request_t wpr; - wpr.target_system = target_systemid; - wpr.target_component = target_compid; - wpr.seq = seq; - mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_waypoint_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - // seq not the second last waypoint - if ((uint16_t)(seq+1) < waypoints->size()) - { - mavlink_waypoint_t *next = waypoints->at(seq+1); - const PxVector3 B(next->x, next->y, next->z); - const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); - if (r >= 0 && r <= 1) - { - const PxVector3 P(A + r*(B-A)); - return (P-C).length(); - } - else if (r < 0.f) - { - return (C-A).length(); - } - else - { - return (C-B).length(); - } - } - else - { - return (C-A).length(); - } - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } - return -1.f; -} - -float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - return (C-A).length(); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } - return -1.f; -} - - -static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user) -{ - // Handle param messages - paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); - current_state = PX_WPP_IDLE; - protocol_current_count = 0; - protocol_current_partner_systemid = 0; - protocol_current_partner_compid = 0; - protocol_current_wp_id = -1; - - if(waypoints->size() == 0) - { - current_active_wp_id = -1; - } - } - - if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size()) - { - send_setpoint(current_active_wp_id); - } - - switch(msg->msgid) - { - case MAVLINK_MSG_ID_ATTITUDE: - { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); - if(wp->frame == 1) - { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE"); - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) - { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - yawReached = true; - } - else if(att.yaw - yaw_tolerance < 0.0f) - { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - yawReached = true; - } - else - { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - yawReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION: - { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 1) - { - mavlink_local_position_t pos; - mavlink_msg_local_position_decode(msg, &pos); - if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); - - posReached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) - { - dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); - } - else - { - dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && yawReached) - { - posReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_CMD: // special action from ground station - { - mavlink_cmd_t action; - mavlink_msg_cmd_decode(msg, &action); - if(action.target == systemid) - { - if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; - switch (action.action) - { - // case MAV_ACTION_LAUNCH: - // if (verbose) std::cerr << "Launch received" << std::endl; - // current_active_wp_id = 0; - // if (waypoints->size()>0) - // { - // setActive(waypoints[current_active_wp_id]); - // } - // else - // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; - // break; - - // case MAV_ACTION_CONTINUE: - // if (verbose) std::c - // err << "Continue received" << std::endl; - // idle = false; - // setActive(waypoints[current_active_wp_id]); - // break; - - // case MAV_ACTION_HALT: - // if (verbose) std::cerr << "Halt received" << std::endl; - // idle = true; - // break; - - // default: - // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; - // break; - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - mavlink_waypoint_ack_t wpa; - mavlink_msg_waypoint_ack_decode(msg, &wpa); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) - { - if (protocol_current_wp_id == waypoints->size()-1) - { - if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); - current_state = PX_WPP_IDLE; - protocol_current_wp_id = 0; - } - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - mavlink_waypoint_set_current_t wpc; - mavlink_msg_waypoint_set_current_decode(msg, &wpc); - - if(wpc.target_system == systemid && wpc.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE) - { - if (wpc.seq < waypoints->size()) - { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); - current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < waypoints->size(); i++) - { - if (i == current_active_wp_id) - { - waypoints->at(i)->current = true; - } - else - { - waypoints->at(i)->current = false; - } - } - if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); - } - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - mavlink_waypoint_request_list_t wprl; - mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == systemid && wprl.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) - { - if (waypoints->size() > 0) - { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); - current_state = PX_WPP_SENDLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - } - else - { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - protocol_current_count = waypoints->size(); - send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - mavlink_waypoint_request_t wpr; - mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) - { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) - { - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - current_state = PX_WPP_SENDLIST_SENDWPS; - protocol_current_wp_id = wpr.seq; - send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); - } - else - { - if (verbose) - { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } - else if (current_state == PX_WPP_SENDLIST) - { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } - else if (current_state == PX_WPP_SENDLIST_SENDWPS) - { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - mavlink_waypoint_count_t wpc; - mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == systemid && wpc.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) - { - if (wpc.count > 0) - { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - current_state = PX_WPP_GETLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - protocol_current_count = wpc.count; - - printf("clearing receive buffer and readying for receiving waypoints\n"); - while(waypoints_receive_buffer->size() > 0) - { - delete waypoints_receive_buffer->back(); - waypoints_receive_buffer->pop_back(); - } - - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - else if (wpc.count == 0) - { - printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n"); - while(waypoints_receive_buffer->size() > 0) - { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - yawReached = false; - posReached = false; - break; - - } - else - { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } - else - { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT: - { - mavlink_waypoint_t wp; - mavlink_msg_waypoint_decode(msg, &wp); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) - { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) - { - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - - current_state = PX_WPP_GETLIST_GETWPS; - protocol_current_wp_id = wp.seq + 1; - mavlink_waypoint_t* newwp = new mavlink_waypoint_t; - memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - waypoints_receive_buffer->push_back(newwp); - - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - - if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) - { - if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); - - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - - if (current_active_wp_id > waypoints_receive_buffer->size()-1) - { - current_active_wp_id = waypoints_receive_buffer->size() - 1; - } - - // switch the waypoints list - std::vector* waypoints_temp = waypoints; - waypoints = waypoints_receive_buffer; - waypoints_receive_buffer = waypoints_temp; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < waypoints->size(); i++) - { - if (waypoints->at(i)->current == 1) - { - current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == waypoints->size()) - { - current_active_wp_id = -1; - yawReached = false; - posReached = false; - timestamp_firstinside_orbit = 0; - } - - current_state = PX_WPP_IDLE; - } - else - { - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - } - else - { - if (current_state == PX_WPP_IDLE) - { - //we're done receiving waypoints, answer with ack. - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) - { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } - else if (current_state == PX_WPP_GETLIST) - { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else if (current_state == PX_WPP_GETLIST_GETWPS) - { - if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); - } - else if(wp.target_system == systemid && wp.target_component == compid) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - mavlink_waypoint_clear_all_t wpca; - mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - - if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) - { - protocol_timestamp_lastaction = now; - - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - while(waypoints->size() > 0) - { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - yawReached = false; - posReached = false; - } - else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); - } - break; - } - - default: - { - if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl; - break; - } - } - - //check if the current waypoint was reached - if ((posReached && /*yawReached &&*/ !idle)) - { - if (current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); - - if (timestamp_firstinside_orbit == 0) - { - // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); - send_waypoint_reached(cur_wp->seq); - timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) - { - if (cur_wp->autocontinue) - { - cur_wp->current = 0; - if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) - { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - current_active_wp_id = 1; - } - else - { - if ((uint16_t)(current_active_wp_id + 1) < waypoints->size()) - current_active_wp_id++; - } - - // Fly to next waypoint - timestamp_firstinside_orbit = 0; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - waypoints->at(current_active_wp_id)->current = true; - posReached = false; - yawReached = false; - if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); - } - } - } - } - else - { - timestamp_lastoutside_orbit = now; - } -} - - - - - - - - -int main(int argc, char* argv[]) -{ - - char help[] = "--help"; - - - char target_ip[100]; - - float position[6] = {}; - int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - struct sockaddr_in gcAddr; - struct sockaddr_in locAddr; - //struct sockaddr_in fromAddr; - uint8_t buf[BUFFER_LENGTH]; - ssize_t recsize; - socklen_t fromlen; - int bytes_sent; - mavlink_message_t msg; - uint16_t len; - int i = 0; - //int success = 0; - unsigned int temp = 0; - - // Check if --help flag was used - if ((argc == 2) && (strcmp(argv[1], help) == 0)) - { - printf("\n"); - printf("\tUsage:\n\n"); - printf("\t"); - printf("%s", argv[0]); - printf(" \n"); - printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); - exit(EXIT_FAILURE); - } - - - // Change the target ip if parameter was given - strcpy(target_ip, "127.0.0.1"); - if (argc == 2) - { - strcpy(target_ip, argv[1]); - } - - - memset(&locAddr, 0, sizeof(locAddr)); - locAddr.sin_family = AF_INET; - locAddr.sin_addr.s_addr = INADDR_ANY; - locAddr.sin_port = htons(14551); - - /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ - if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) - { - perror("error bind failed"); - close(sock); - exit(EXIT_FAILURE); - } - - /* Attempt to make it non blocking */ - if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) - { - fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); - close(sock); - exit(EXIT_FAILURE); - } - - - memset(&gcAddr, 0, sizeof(gcAddr)); - gcAddr.sin_family = AF_INET; - gcAddr.sin_addr.s_addr = inet_addr(target_ip); - gcAddr.sin_port = htons(14550); - - - printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); - - - for (;;) - { - - /*Send Heartbeat */ - mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send Status */ - mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - /* Send Local Position */ - mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), - position[0], position[1], position[2], - position[3], position[4], position[5]); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send attitude */ - mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - - memset(buf, 0, BUFFER_LENGTH); - recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); - if (recsize > 0) - { - // Something received - print out all bytes and parse packet - mavlink_message_t msg; - mavlink_status_t status; - - printf("Bytes Received: %d\nDatagram: ", (int)recsize); - for (i = 0; i < recsize; ++i) - { - temp = buf[i]; - printf("%02x ", (unsigned char)temp); - if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) - { - // Packet received - printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); - } - } - printf("\n"); - } - memset(buf, 0, BUFFER_LENGTH); - sleep(1); // Sleep one second - } -} - - -/* QNX timer version */ -#if (defined __QNX__) | (defined __QNXNTO__) -uint64_t microsSinceEpoch() -{ - - struct timespec time; - - uint64_t micros = 0; - - clock_gettime(CLOCK_REALTIME, &time); - micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; - - return micros; -} -#else -uint64_t microsSinceEpoch() -{ - - struct timeval tv; - - uint64_t micros = 0; - - gettimeofday(&tv, NULL); - micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - - return micros; -} -#endif \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/testing/udptest.1 b/src/GCS_MAVLink/missionlib/testing/udptest.1 deleted file mode 100644 index 78b31915f..000000000 --- a/src/GCS_MAVLink/missionlib/testing/udptest.1 +++ /dev/null @@ -1,79 +0,0 @@ -.\"Modified from man(1) of FreeBSD, the NetBSD mdoc.template, and mdoc.samples. -.\"See Also: -.\"man mdoc.samples for a complete listing of options -.\"man mdoc for the short list of editing options -.\"/usr/share/misc/mdoc.template -.Dd 01.08.11 \" DATE -.Dt udptest 1 \" Program name and manual section number -.Os Darwin -.Sh NAME \" Section Header - required - don't modify -.Nm udptest, -.\" The following lines are read in generating the apropos(man -k) database. Use only key -.\" words here as the database is built based on the words here and in the .ND line. -.Nm Other_name_for_same_program(), -.Nm Yet another name for the same program. -.\" Use .Nm macro to designate other names for the documented program. -.Nd This line parsed for whatis database. -.Sh SYNOPSIS \" Section Header - required - don't modify -.Nm -.Op Fl abcd \" [-abcd] -.Op Fl a Ar path \" [-a path] -.Op Ar file \" [file] -.Op Ar \" [file ...] -.Ar arg0 \" Underlined argument - use .Ar anywhere to underline -arg2 ... \" Arguments -.Sh DESCRIPTION \" Section Header - required - don't modify -Use the .Nm macro to refer to your program throughout the man page like such: -.Nm -Underlining is accomplished with the .Ar macro like this: -.Ar underlined text . -.Pp \" Inserts a space -A list of items with descriptions: -.Bl -tag -width -indent \" Begins a tagged list -.It item a \" Each item preceded by .It macro -Description of item a -.It item b -Description of item b -.El \" Ends the list -.Pp -A list of flags and their descriptions: -.Bl -tag -width -indent \" Differs from above in tag removed -.It Fl a \"-a flag as a list item -Description of -a flag -.It Fl b -Description of -b flag -.El \" Ends the list -.Pp -.\" .Sh ENVIRONMENT \" May not be needed -.\" .Bl -tag -width "ENV_VAR_1" -indent \" ENV_VAR_1 is width of the string ENV_VAR_1 -.\" .It Ev ENV_VAR_1 -.\" Description of ENV_VAR_1 -.\" .It Ev ENV_VAR_2 -.\" Description of ENV_VAR_2 -.\" .El -.Sh FILES \" File used or created by the topic of the man page -.Bl -tag -width "/Users/joeuser/Library/really_long_file_name" -compact -.It Pa /usr/share/file_name -FILE_1 description -.It Pa /Users/joeuser/Library/really_long_file_name -FILE_2 description -.El \" Ends the list -.\" .Sh DIAGNOSTICS \" May not be needed -.\" .Bl -diag -.\" .It Diagnostic Tag -.\" Diagnostic informtion here. -.\" .It Diagnostic Tag -.\" Diagnostic informtion here. -.\" .El -.Sh SEE ALSO -.\" List links in ascending order by section, alphabetically within a section. -.\" Please do not reference files that do not exist without filing a bug report -.Xr a 1 , -.Xr b 1 , -.Xr c 1 , -.Xr a 2 , -.Xr b 2 , -.Xr a 3 , -.Xr b 3 -.\" .Sh BUGS \" Document known, unremedied bugs -.\" .Sh HISTORY \" Document history if command behaves in a unique manner \ No newline at end of file diff --git a/src/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/.gitignore b/src/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/.gitignore deleted file mode 100644 index d82a5e47d..000000000 --- a/src/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*.mode1v3 -*.pbxuser diff --git a/src/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj b/src/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj deleted file mode 100644 index 7bc76570d..000000000 --- a/src/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj +++ /dev/null @@ -1,222 +0,0 @@ -// !$*UTF8*$! -{ - archiveVersion = 1; - classes = { - }; - objectVersion = 45; - objects = { - -/* Begin PBXBuildFile section */ - 348868D013F512010005F759 /* mavlink_parameters.c in Sources */ = {isa = PBXBuildFile; fileRef = 348868CF13F512010005F759 /* mavlink_parameters.c */; }; - 34E8AFDB13F5064C001100AA /* waypoints.c in Sources */ = {isa = PBXBuildFile; fileRef = 34E8AFDA13F5064C001100AA /* waypoints.c */; }; - 8DD76FAC0486AB0100D96B5E /* main.c in Sources */ = {isa = PBXBuildFile; fileRef = 08FB7796FE84155DC02AAC07 /* main.c */; settings = {ATTRIBUTES = (); }; }; -/* End PBXBuildFile section */ - -/* Begin PBXCopyFilesBuildPhase section */ - 8DD76FAF0486AB0100D96B5E /* CopyFiles */ = { - isa = PBXCopyFilesBuildPhase; - buildActionMask = 8; - dstPath = /usr/share/man/man1/; - dstSubfolderSpec = 0; - files = ( - ); - runOnlyForDeploymentPostprocessing = 1; - }; -/* End PBXCopyFilesBuildPhase section */ - -/* Begin PBXFileReference section */ - 08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = ""; }; - 348868CF13F512010005F759 /* mavlink_parameters.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = mavlink_parameters.c; path = ../mavlink_parameters.c; sourceTree = SOURCE_ROOT; }; - 348868D113F512080005F759 /* mavlink_parameters.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = mavlink_parameters.h; path = ../mavlink_parameters.h; sourceTree = SOURCE_ROOT; }; - 34E8AFDA13F5064C001100AA /* waypoints.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = waypoints.c; path = ../waypoints.c; sourceTree = SOURCE_ROOT; }; - 34E8AFDC13F50659001100AA /* waypoints.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = waypoints.h; path = ../waypoints.h; sourceTree = SOURCE_ROOT; }; - 8DD76FB20486AB0100D96B5E /* udptest */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = udptest; sourceTree = BUILT_PRODUCTS_DIR; }; -/* End PBXFileReference section */ - -/* Begin PBXFrameworksBuildPhase section */ - 8DD76FAD0486AB0100D96B5E /* Frameworks */ = { - isa = PBXFrameworksBuildPhase; - buildActionMask = 2147483647; - files = ( - ); - runOnlyForDeploymentPostprocessing = 0; - }; -/* End PBXFrameworksBuildPhase section */ - -/* Begin PBXGroup section */ - 08FB7794FE84155DC02AAC07 /* udptest */ = { - isa = PBXGroup; - children = ( - 34E8AFDC13F50659001100AA /* waypoints.h */, - 08FB7795FE84155DC02AAC07 /* Source */, - C6A0FF2B0290797F04C91782 /* Documentation */, - 1AB674ADFE9D54B511CA2CBB /* Products */, - ); - name = udptest; - sourceTree = ""; - }; - 08FB7795FE84155DC02AAC07 /* Source */ = { - isa = PBXGroup; - children = ( - 348868D113F512080005F759 /* mavlink_parameters.h */, - 348868CF13F512010005F759 /* mavlink_parameters.c */, - 34E8AFDA13F5064C001100AA /* waypoints.c */, - 08FB7796FE84155DC02AAC07 /* main.c */, - ); - name = Source; - sourceTree = ""; - }; - 1AB674ADFE9D54B511CA2CBB /* Products */ = { - isa = PBXGroup; - children = ( - 8DD76FB20486AB0100D96B5E /* udptest */, - ); - name = Products; - sourceTree = ""; - }; - C6A0FF2B0290797F04C91782 /* Documentation */ = { - isa = PBXGroup; - children = ( - ); - name = Documentation; - sourceTree = ""; - }; -/* End PBXGroup section */ - -/* Begin PBXNativeTarget section */ - 8DD76FA90486AB0100D96B5E /* udptest */ = { - isa = PBXNativeTarget; - buildConfigurationList = 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */; - buildPhases = ( - 8DD76FAB0486AB0100D96B5E /* Sources */, - 8DD76FAD0486AB0100D96B5E /* Frameworks */, - 8DD76FAF0486AB0100D96B5E /* CopyFiles */, - ); - buildRules = ( - ); - dependencies = ( - ); - name = udptest; - productInstallPath = "$(HOME)/bin"; - productName = udptest; - productReference = 8DD76FB20486AB0100D96B5E /* udptest */; - productType = "com.apple.product-type.tool"; - }; -/* End PBXNativeTarget section */ - -/* Begin PBXProject section */ - 08FB7793FE84155DC02AAC07 /* Project object */ = { - isa = PBXProject; - buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */; - compatibilityVersion = "Xcode 3.1"; - developmentRegion = English; - hasScannedForEncodings = 1; - knownRegions = ( - English, - Japanese, - French, - German, - ); - mainGroup = 08FB7794FE84155DC02AAC07 /* udptest */; - projectDirPath = ""; - projectRoot = ""; - targets = ( - 8DD76FA90486AB0100D96B5E /* udptest */, - ); - }; -/* End PBXProject section */ - -/* Begin PBXSourcesBuildPhase section */ - 8DD76FAB0486AB0100D96B5E /* Sources */ = { - isa = PBXSourcesBuildPhase; - buildActionMask = 2147483647; - files = ( - 8DD76FAC0486AB0100D96B5E /* main.c in Sources */, - 34E8AFDB13F5064C001100AA /* waypoints.c in Sources */, - 348868D013F512010005F759 /* mavlink_parameters.c in Sources */, - ); - runOnlyForDeploymentPostprocessing = 0; - }; -/* End PBXSourcesBuildPhase section */ - -/* Begin XCBuildConfiguration section */ - 1DEB928608733DD80010E9CD /* Debug */ = { - isa = XCBuildConfiguration; - buildSettings = { - ALWAYS_SEARCH_USER_PATHS = NO; - COPY_PHASE_STRIP = NO; - GCC_DYNAMIC_NO_PIC = NO; - GCC_ENABLE_FIX_AND_CONTINUE = YES; - GCC_MODEL_TUNING = G5; - GCC_OPTIMIZATION_LEVEL = 0; - INSTALL_PATH = /usr/local/bin; - PRODUCT_NAME = udptest; - }; - name = Debug; - }; - 1DEB928708733DD80010E9CD /* Release */ = { - isa = XCBuildConfiguration; - buildSettings = { - ALWAYS_SEARCH_USER_PATHS = NO; - DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym"; - GCC_MODEL_TUNING = G5; - INSTALL_PATH = /usr/local/bin; - PRODUCT_NAME = udptest; - }; - name = Release; - }; - 1DEB928A08733DD80010E9CD /* Debug */ = { - isa = XCBuildConfiguration; - buildSettings = { - ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; - GCC_C_LANGUAGE_STANDARD = gnu99; - GCC_OPTIMIZATION_LEVEL = 0; - GCC_WARN_ABOUT_RETURN_TYPE = YES; - GCC_WARN_UNUSED_VARIABLE = YES; - HEADER_SEARCH_PATHS = ( - ../../include/, - ../../include/common/, - ); - ONLY_ACTIVE_ARCH = YES; - PREBINDING = NO; - SDKROOT = macosx10.6; - }; - name = Debug; - }; - 1DEB928B08733DD80010E9CD /* Release */ = { - isa = XCBuildConfiguration; - buildSettings = { - ARCHS = "$(ARCHS_STANDARD_32_64_BIT)"; - GCC_C_LANGUAGE_STANDARD = gnu99; - GCC_WARN_ABOUT_RETURN_TYPE = YES; - GCC_WARN_UNUSED_VARIABLE = YES; - PREBINDING = NO; - SDKROOT = macosx10.6; - }; - name = Release; - }; -/* End XCBuildConfiguration section */ - -/* Begin XCConfigurationList section */ - 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */ = { - isa = XCConfigurationList; - buildConfigurations = ( - 1DEB928608733DD80010E9CD /* Debug */, - 1DEB928708733DD80010E9CD /* Release */, - ); - defaultConfigurationIsVisible = 0; - defaultConfigurationName = Release; - }; - 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */ = { - isa = XCConfigurationList; - buildConfigurations = ( - 1DEB928A08733DD80010E9CD /* Debug */, - 1DEB928B08733DD80010E9CD /* Release */, - ); - defaultConfigurationIsVisible = 0; - defaultConfigurationName = Release; - }; -/* End XCConfigurationList section */ - }; - rootObject = 08FB7793FE84155DC02AAC07 /* Project object */; -} diff --git a/src/GCS_MAVLink/missionlib/waypoints.c b/src/GCS_MAVLink/missionlib/waypoints.c deleted file mode 100644 index 44bddb6f5..000000000 --- a/src/GCS_MAVLink/missionlib/waypoints.c +++ /dev/null @@ -1,1028 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ - -#include "waypoints.h" -#include - -bool debug = true; -bool verbose = true; - -extern mavlink_wpm_storage wpm; - -extern void mavlink_missionlib_send_message(mavlink_message_t* msg); -extern void mavlink_missionlib_send_gcs_string(const char* string); -extern uint64_t mavlink_missionlib_get_system_timestamp(); -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - - -#define MAVLINK_WPM_NO_PRINTF - -uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - -void mavlink_wpm_init(mavlink_wpm_storage* state) -{ - // Set all waypoints to zero - - // Set count to zero - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - -} - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = wpm.current_partner_sysid; - wpa.target_component = wpm.current_partner_compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -#endif - mavlink_missionlib_send_gcs_string("Sent waypoint ACK"); - } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if(seq < wpm.size) - { - mavlink_mission_item_t *cur = &(wpm.waypoints[seq]); - - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Set current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if(seq < wpm.size) - { - mavlink_mission_item_t *cur = &(wpm.waypoints[seq]); - mavlink_missionlib_current_waypoint_changed(cur->seq, cur->param1, - cur->param2, cur->param3, cur->param4, cur->x, - cur->y, cur->z, cur->frame, cur->command); - - wpm.timestamp_last_send_setpoint = mavlink_missionlib_get_system_timestamp(); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = wpm.current_partner_sysid; - wpc.target_component = wpm.current_partner_compid; - wpc.count = count; - - mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm.size) - { - mavlink_message_t msg; - mavlink_mission_item_t *wp = &(wpm.waypoints[seq]); - wp->target_system = wpm.current_partner_sysid; - wp->target_component = wpm.current_partner_compid; - mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); - mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm.max_size) - { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = wpm.current_partner_sysid; - wpr.target_component = wpm.current_partner_compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_missionlib_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm.size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm.size) -// { -// mavlink_mission_item_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - -float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) -{ -// if (seq < wpm.size) -// { -// mavlink_mission_item_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// return (C-A).length(); -// } -// else -// { -// // if (verbose) // printf("ERROR: index out of bounds\n"); -// } - return -1.f; -} - -void mavlink_wpm_loop() -{ - //check for timed-out operations - uint64_t now = mavlink_missionlib_get_system_timestamp(); - if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Operation timeout switching -> IDLE"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); -#endif - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - wpm.current_count = 0; - wpm.current_partner_sysid = 0; - wpm.current_partner_compid = 0; - wpm.current_wp_id = -1; - - if(wpm.size == 0) - { - wpm.current_active_wp_id = -1; - } - } - - if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) - { - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - } -} - -void mavlink_wpm_message_handler(const mavlink_message_t* msg) -{ - uint64_t now = mavlink_missionlib_get_system_timestamp(); - switch(msg->msgid) - { - case MAVLINK_MSG_ID_ATTITUDE: - { - if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) - { - mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); - if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) - { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = wpm.accept_range_yaw; - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) - { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - wpm.yaw_reached = true; - } - else if(att.yaw - yaw_tolerance < 0.0f) - { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - wpm.yaw_reached = true; - } - else - { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - wpm.yaw_reached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION_NED: - { - if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) - { - mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); - - if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) - { - mavlink_local_position_ned_t pos; - mavlink_msg_local_position_ned_decode(msg, &pos); - //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); - - wpm.pos_reached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; -// if (wp->param2 == 0) -// { -// // FIXME segment distance -// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); -// } -// else -// { - dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); -// } - - if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) - { - wpm.pos_reached = true; - } - } - } - break; - } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm.size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } - - case MAVLINK_MSG_ID_MISSION_ACK: - { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) - { - if (wpm.current_wp_id == wpm.size-1) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); -#endif - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - wpm.current_wp_id = 0; - } - } - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); -#endif - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: - { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - if (wpc.seq < wpm.size) - { - // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); - wpm.current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < wpm.size; i++) - { - if (i == wpm.current_active_wp_id) - { - wpm.waypoints[i].current = true; - } - else - { - wpm.waypoints[i].current = false; - } - } -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("NEW WP SET"); -#else - if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id); -#endif - wpm.yaw_reached = false; - wpm.pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.timestamp_firstinside_orbit = 0; - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); -#endif - } - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); -#endif - } - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); -#endif - } - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { - if (wpm.size > 0) - { - //if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); -// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm.current_wp_id = 0; - wpm.current_partner_sysid = msg->sysid; - wpm.current_partner_compid = msg->compid; - } - else - { - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - wpm.current_count = wpm.size; - mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); - } - else - { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); - } - } - else - { - // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: - { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) - { - wpm.timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) - { - if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); -#endif - } - if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); -#endif - } - if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); -#endif - } - - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm.current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); - } - else - { - // if (verbose) - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); -#endif - break; - } - else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { - if (wpr.seq != 0) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); -#endif - } - } - else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) - { - if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); -#endif - } - else if (wpr.seq >= wpm.size) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); -#endif - } - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); -#endif - } - } - } - } - else - { - //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); -#endif - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); -#endif - } - - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: - { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) - { - if (wpc.count > 0) - { - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); -#endif - } - if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); -#endif - } - - wpm.current_state = MAVLINK_WPM_STATE_GETLIST; - wpm.current_wp_id = 0; - wpm.current_partner_sysid = msg->sysid; - wpm.current_partner_compid = msg->compid; - wpm.current_count = wpc.count; - -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); -#else - if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); -#endif - wpm.rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } - - mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); - } - else if (wpc.count == 0) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("COUNT 0"); -#else - if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); -#endif - wpm.rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - break; - - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("IGN WP CMD"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); -#endif - } - } - else - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); -#endif - } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); -#endif - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); -#else - if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); -#endif - } - } - } - else - { -#ifdef MAVLINK_WPM_NO_PRINTF - mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); -#else - if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); -#endif - } - - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: - { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - mavlink_missionlib_send_gcs_string("GOT WP"); - - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) - { - wpm.timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) - { -// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); -// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); -// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); -// - wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_mission_item_t* newwp = &(wpm.rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); - wpm.current_wp_id = wp.seq + 1; - - // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - - if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) - { - mavlink_missionlib_send_gcs_string("GOT ALL WPS"); - // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); - - mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - - if (wpm.current_active_wp_id > wpm.rcv_size-1) - { - wpm.current_active_wp_id = wpm.rcv_size-1; - } - - // switch the waypoints list - // FIXME CHECK!!! - for (int i = 0; i < wpm.current_count; ++i) - { - wpm.waypoints[i] = wpm.rcv_waypoints[i]; - } - wpm.size = wpm.current_count; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < wpm.size; i++) - { - if (wpm.waypoints[i].current == 1) - { - wpm.current_active_wp_id = i; - //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); - wpm.yaw_reached = false; - wpm.pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == wpm.size) - { - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - wpm.timestamp_firstinside_orbit = 0; - } - - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - } - else - { - mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); - } - } - else - { - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - // printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - // if (verbose) - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); - break; - } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) - { - if(!(wp.seq == 0)) - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); - } - else - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) - { - if (!(wp.seq == wpm.current_wp_id)) - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); - } - else if (!(wp.seq < wpm.current_count)) - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); - } - else - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - else - { - // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); - } - } - } - } - else - { - //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); - } - else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) - { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - wpm.timestamp_lastaction = now; - - // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm.size = 0; - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - } - else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); - } - break; - } - - default: - { - // if (debug) // printf("Waypoint: received message of unknown type"); - break; - } - } - - //check if the current waypoint was reached - if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) - { - if (wpm.current_active_wp_id < wpm.size) - { - mavlink_mission_item_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); - - if (wpm.timestamp_firstinside_orbit == 0) - { - // Announce that last waypoint was reached - // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq); - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm.timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) - { - if (cur_wp->autocontinue) - { - cur_wp->current = 0; - if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) - { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - wpm.current_active_wp_id = 1; - } - else - { - if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) - wpm.current_active_wp_id++; - } - - // Fly to next waypoint - wpm.timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.waypoints[wpm.current_active_wp_id].current = true; - wpm.pos_reached = false; - wpm.yaw_reached = false; - // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); - } - } - } - } - else - { - wpm.timestamp_lastoutside_orbit = now; - } -} - diff --git a/src/GCS_MAVLink/missionlib/waypoints.h b/src/GCS_MAVLink/missionlib/waypoints.h deleted file mode 100644 index 49374f6a3..000000000 --- a/src/GCS_MAVLink/missionlib/waypoints.h +++ /dev/null @@ -1,104 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -// Disable auto-data structures -#ifndef MAVLINK_NO_DATA -#define MAVLINK_NO_DATA -#endif - -#include -extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t* buffer, uint16_t len); - -#ifndef MAVLINK_SEND_UART_BYTES -#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len) -#endif -extern mavlink_system_t mavlink_system; -#include -#include - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES -{ - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES -{ - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 15 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#ifndef MAVLINK_WPM_TEXT_FEEDBACK -#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text -#endif -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000 ///< Protocol communication timeout in milliseconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 - - -struct mavlink_wpm_storage { - mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t size; - uint16_t max_size; - uint16_t rcv_size; - enum MAVLINK_WPM_STATES current_state; - uint16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; - uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -void mavlink_wpm_init(mavlink_wpm_storage* state); -void mavlink_wpm_loop(); -void mavlink_wpm_message_handler(const mavlink_message_t* msg);