diff options
Diffstat (limited to 'mavlink/include/mavlink/v0.9/common')
74 files changed, 20414 insertions, 0 deletions
diff --git a/mavlink/include/mavlink/v0.9/common/common.h b/mavlink/include/mavlink/v0.9/common/common.h new file mode 100644 index 000000000..84538ed57 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/common.h @@ -0,0 +1,208 @@ +/** @file + * @brief MAVLink comm protocol generated from common.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef COMMON_H +#define COMMON_H + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#endif + +#ifndef MAVLINK_MESSAGE_INFO +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_COMMON + + + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 2 +#endif + +// ENUM DEFINITIONS + + +/** @brief Commands to be executed by the MAV. They can be executed on user request, + or as part of a mission script. If the action is used in a mission, the parameter mapping + to the waypoint/mission message is as follows: + Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what + ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ +#ifndef HAVE_ENUM_MAV_CMD +#define HAVE_ENUM_MAV_CMD +enum MAV_CMD +{ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at waypoint (rotary wing)| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time |Empty| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Turns| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds |Seconds (decimal)| Empty| Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| */ + MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ + MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + devices such as cameras. + |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty| */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ + MAV_CMD_ENUM_END=246, /* | */ +}; +#endif + +/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. + */ +#ifndef HAVE_ENUM_MAV_DATA_STREAM +#define HAVE_ENUM_MAV_DATA_STREAM +enum MAV_DATA_STREAM +{ + MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ENUM_END=13, /* | */ +}; +#endif + +/** @brief The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). + */ +#ifndef HAVE_ENUM_MAV_ROI +#define HAVE_ENUM_MAV_ROI +enum MAV_ROI +{ + MAV_ROI_NONE=0, /* No region of interest. | */ + MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=3, /* Point toward fixed location. | */ + MAV_ROI_TARGET=4, /* Point toward of given id. | */ + MAV_ROI_ENUM_END=5, /* | */ +}; +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_heartbeat.h" +#include "./mavlink_msg_boot.h" +#include "./mavlink_msg_system_time.h" +#include "./mavlink_msg_ping.h" +#include "./mavlink_msg_system_time_utc.h" +#include "./mavlink_msg_change_operator_control.h" +#include "./mavlink_msg_change_operator_control_ack.h" +#include "./mavlink_msg_auth_key.h" +#include "./mavlink_msg_action_ack.h" +#include "./mavlink_msg_action.h" +#include "./mavlink_msg_set_mode.h" +#include "./mavlink_msg_set_nav_mode.h" +#include "./mavlink_msg_param_request_read.h" +#include "./mavlink_msg_param_request_list.h" +#include "./mavlink_msg_param_value.h" +#include "./mavlink_msg_param_set.h" +#include "./mavlink_msg_gps_raw_int.h" +#include "./mavlink_msg_scaled_imu.h" +#include "./mavlink_msg_gps_status.h" +#include "./mavlink_msg_raw_imu.h" +#include "./mavlink_msg_raw_pressure.h" +#include "./mavlink_msg_scaled_pressure.h" +#include "./mavlink_msg_attitude.h" +#include "./mavlink_msg_local_position.h" +#include "./mavlink_msg_global_position.h" +#include "./mavlink_msg_gps_raw.h" +#include "./mavlink_msg_sys_status.h" +#include "./mavlink_msg_rc_channels_raw.h" +#include "./mavlink_msg_rc_channels_scaled.h" +#include "./mavlink_msg_servo_output_raw.h" +#include "./mavlink_msg_waypoint.h" +#include "./mavlink_msg_waypoint_request.h" +#include "./mavlink_msg_waypoint_set_current.h" +#include "./mavlink_msg_waypoint_current.h" +#include "./mavlink_msg_waypoint_request_list.h" +#include "./mavlink_msg_waypoint_count.h" +#include "./mavlink_msg_waypoint_clear_all.h" +#include "./mavlink_msg_waypoint_reached.h" +#include "./mavlink_msg_waypoint_ack.h" +#include "./mavlink_msg_gps_set_global_origin.h" +#include "./mavlink_msg_gps_local_origin_set.h" +#include "./mavlink_msg_local_position_setpoint_set.h" +#include "./mavlink_msg_local_position_setpoint.h" +#include "./mavlink_msg_control_status.h" +#include "./mavlink_msg_safety_set_allowed_area.h" +#include "./mavlink_msg_safety_allowed_area.h" +#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" +#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" +#include "./mavlink_msg_nav_controller_output.h" +#include "./mavlink_msg_position_target.h" +#include "./mavlink_msg_state_correction.h" +#include "./mavlink_msg_set_altitude.h" +#include "./mavlink_msg_request_data_stream.h" +#include "./mavlink_msg_hil_state.h" +#include "./mavlink_msg_hil_controls.h" +#include "./mavlink_msg_manual_control.h" +#include "./mavlink_msg_rc_channels_override.h" +#include "./mavlink_msg_global_position_int.h" +#include "./mavlink_msg_vfr_hud.h" +#include "./mavlink_msg_command.h" +#include "./mavlink_msg_command_ack.h" +#include "./mavlink_msg_optical_flow.h" +#include "./mavlink_msg_object_detection_event.h" +#include "./mavlink_msg_debug_vect.h" +#include "./mavlink_msg_named_value_float.h" +#include "./mavlink_msg_named_value_int.h" +#include "./mavlink_msg_statustext.h" +#include "./mavlink_msg_debug.h" + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // COMMON_H diff --git a/mavlink/include/mavlink/v0.9/common/mavlink.h b/mavlink/include/mavlink/v0.9/common/mavlink.h new file mode 100644 index 000000000..02ff5bd39 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink.h @@ -0,0 +1,27 @@ +/** @file + * @brief MAVLink comm protocol built from common.xml + * @see http://pixhawk.ethz.ch/software/mavlink + */ +#ifndef MAVLINK_H +#define MAVLINK_H + +#ifndef MAVLINK_STX +#define MAVLINK_STX 85 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_BIG_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 0 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 0 +#endif + +#include "version.h" +#include "common.h" + +#endif // MAVLINK_H diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_action.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_action.h new file mode 100644 index 000000000..ada9aa7a2 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_action.h @@ -0,0 +1,188 @@ +// 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_NON_CONST(msg), buf, 3); +#else + mavlink_action_t packet; + packet.target = target; + packet.target_component = target_component; + packet.action = action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTION; + return mavlink_finalize_message(msg, system_id, component_id, 3); +} + +/** + * @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_NON_CONST(msg), buf, 3); +#else + mavlink_action_t packet; + packet.target = target; + packet.target_component = target_component; + packet.action = action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_action_ack.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_action_ack.h new file mode 100644 index 000000000..a87b35b59 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_action_ack.h @@ -0,0 +1,166 @@ +// 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_NON_CONST(msg), buf, 2); +#else + mavlink_action_ack_t packet; + packet.action = action; + packet.result = result; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @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_NON_CONST(msg), buf, 2); +#else + mavlink_action_ack_t packet; + packet.action = action; + packet.result = result; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_attitude.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_attitude.h new file mode 100644 index 000000000..188f1eb0f --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_attitude.h @@ -0,0 +1,276 @@ +// MESSAGE ATTITUDE PACKING + +#define MAVLINK_MSG_ID_ATTITUDE 30 + +typedef struct __mavlink_attitude_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) +} mavlink_attitude_t; + +#define MAVLINK_MSG_ID_ATTITUDE_LEN 32 +#define MAVLINK_MSG_ID_30_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_ATTITUDE { \ + "ATTITUDE", \ + 7, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_attitude_t, usec) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, yaw) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_t, yawspeed) }, \ + } \ +} + + +/** + * @brief Pack a attitude 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 roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); +#else + mavlink_attitude_t packet; + packet.usec = usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, 32); +} + +/** + * @brief Pack a attitude 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 roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); +#else + mavlink_attitude_t packet; + packet.usec = usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); +} + +/** + * @brief Encode a attitude 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 attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_t* attitude) +{ + return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->usec, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); +} + +/** + * @brief Send a attitude message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, 32); +#else + mavlink_attitude_t packet; + packet.usec = usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, 32); +#endif +} + +#endif + +// MESSAGE ATTITUDE UNPACKING + + +/** + * @brief Get field usec from attitude message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_attitude_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll from attitude message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch from attitude message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw from attitude message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rollspeed from attitude message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitchspeed from attitude message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yawspeed from attitude message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a attitude message into a struct + * + * @param msg The message to decode + * @param attitude C-struct to decode the message contents into + */ +static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP + attitude->usec = mavlink_msg_attitude_get_usec(msg); + attitude->roll = mavlink_msg_attitude_get_roll(msg); + attitude->pitch = mavlink_msg_attitude_get_pitch(msg); + attitude->yaw = mavlink_msg_attitude_get_yaw(msg); + attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg); + attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); + attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); +#else + memcpy(attitude, _MAV_PAYLOAD(msg), 32); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_auth_key.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_auth_key.h new file mode 100644 index 000000000..c451444ea --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_auth_key.h @@ -0,0 +1,144 @@ +// MESSAGE AUTH_KEY PACKING + +#define MAVLINK_MSG_ID_AUTH_KEY 7 + +typedef struct __mavlink_auth_key_t +{ + char key[32]; ///< key +} mavlink_auth_key_t; + +#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 +#define MAVLINK_MSG_ID_7_LEN 32 + +#define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 + +#define MAVLINK_MESSAGE_INFO_AUTH_KEY { \ + "AUTH_KEY", \ + 1, \ + { { "key", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ + } \ +} + + +/** + * @brief Pack a auth_key message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; + return mavlink_finalize_message(msg, system_id, component_id, 32); +} + +/** + * @brief Pack a auth_key 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 key key + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); +} + +/** + * @brief Encode a auth_key struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param auth_key C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_auth_key_t* auth_key) +{ + return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); +} + +/** + * @brief Send a auth_key message + * @param chan MAVLink channel to send the message + * + * @param key key + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + + _mav_put_char_array(buf, 0, key, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, 32); +#else + mavlink_auth_key_t packet; + + mav_array_memcpy(packet.key, key, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, 32); +#endif +} + +#endif + +// MESSAGE AUTH_KEY UNPACKING + + +/** + * @brief Get field key from auth_key message + * + * @return key + */ +static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) +{ + return _MAV_RETURN_char_array(msg, key, 32, 0); +} + +/** + * @brief Decode a auth_key message into a struct + * + * @param msg The message to decode + * @param auth_key C-struct to decode the message contents into + */ +static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mavlink_auth_key_t* auth_key) +{ +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_auth_key_get_key(msg, auth_key->key); +#else + memcpy(auth_key, _MAV_PAYLOAD(msg), 32); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_boot.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_boot.h new file mode 100644 index 000000000..570949bd5 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_boot.h @@ -0,0 +1,144 @@ +// 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_NON_CONST(msg), buf, 4); +#else + mavlink_boot_t packet; + packet.version = version; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_BOOT; + return mavlink_finalize_message(msg, system_id, component_id, 4); +} + +/** + * @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_NON_CONST(msg), buf, 4); +#else + mavlink_boot_t packet; + packet.version = version; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_BOOT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); +} + +/** + * @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); +#else + mavlink_boot_t packet; + packet.version = version; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BOOT, (const char *)&packet, 4); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_change_operator_control.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_change_operator_control.h new file mode 100644 index 000000000..8fad932ea --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_change_operator_control.h @@ -0,0 +1,204 @@ +// MESSAGE CHANGE_OPERATOR_CONTROL PACKING + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 + +typedef struct __mavlink_change_operator_control_t +{ + uint8_t target_system; ///< System the GCS requests control for + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" +} mavlink_change_operator_control_t; + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 +#define MAVLINK_MSG_ID_5_LEN 28 + +#define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 + +#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL { \ + "CHANGE_OPERATOR_CONTROL", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \ + { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \ + { "version", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \ + { "passkey", NULL, MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \ + } \ +} + + +/** + * @brief Pack a change_operator_control 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 the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, 28); +} + +/** + * @brief Pack a change_operator_control 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 the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28); +} + +/** + * @brief Encode a change_operator_control 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 change_operator_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_t* change_operator_control) +{ + return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); +} + +/** + * @brief Send a change_operator_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System the GCS requests control for + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char *passkey) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + mav_array_memcpy(packet.passkey, passkey, sizeof(char)*25); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, 28); +#endif +} + +#endif + +// MESSAGE CHANGE_OPERATOR_CONTROL UNPACKING + + +/** + * @brief Get field target_system from change_operator_control message + * + * @return System the GCS requests control for + */ +static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field control_request from change_operator_control message + * + * @return 0: request control of this MAV, 1: Release control of this MAV + */ +static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field version from change_operator_control message + * + * @return 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + */ +static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field passkey from change_operator_control message + * + * @return Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + */ +static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char *passkey) +{ + return _MAV_RETURN_char_array(msg, passkey, 25, 3); +} + +/** + * @brief Decode a change_operator_control message into a struct + * + * @param msg The message to decode + * @param change_operator_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_change_operator_control_decode(const mavlink_message_t* msg, mavlink_change_operator_control_t* change_operator_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + change_operator_control->target_system = mavlink_msg_change_operator_control_get_target_system(msg); + change_operator_control->control_request = mavlink_msg_change_operator_control_get_control_request(msg); + change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg); + mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey); +#else + memcpy(change_operator_control, _MAV_PAYLOAD(msg), 28); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_change_operator_control_ack.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_change_operator_control_ack.h new file mode 100644 index 000000000..e9e195bbb --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_change_operator_control_ack.h @@ -0,0 +1,188 @@ +// MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 + +typedef struct __mavlink_change_operator_control_ack_t +{ + uint8_t gcs_system_id; ///< ID of the GCS this message + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control +} mavlink_change_operator_control_ack_t; + +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 +#define MAVLINK_MSG_ID_6_LEN 3 + + + +#define MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK { \ + "CHANGE_OPERATOR_CONTROL_ACK", \ + 3, \ + { { "gcs_system_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_ack_t, gcs_system_id) }, \ + { "control_request", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_ack_t, control_request) }, \ + { "ack", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_ack_t, ack) }, \ + } \ +} + + +/** + * @brief Pack a change_operator_control_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 gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + return mavlink_finalize_message(msg, system_id, component_id, 3); +} + +/** + * @brief Pack a change_operator_control_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 gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t gcs_system_id,uint8_t control_request,uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); +} + +/** + * @brief Encode a change_operator_control_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 change_operator_control_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ + return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); +} + +/** + * @brief Send a change_operator_control_ack message + * @param chan MAVLink channel to send the message + * + * @param gcs_system_id ID of the GCS this message + * @param control_request 0: request control of this MAV, 1: Release control of this MAV + * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, 3); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, 3); +#endif +} + +#endif + +// MESSAGE CHANGE_OPERATOR_CONTROL_ACK UNPACKING + + +/** + * @brief Get field gcs_system_id from change_operator_control_ack message + * + * @return ID of the GCS this message + */ +static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field control_request from change_operator_control_ack message + * + * @return 0: request control of this MAV, 1: Release control of this MAV + */ +static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field ack from change_operator_control_ack message + * + * @return 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + */ +static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a change_operator_control_ack message into a struct + * + * @param msg The message to decode + * @param change_operator_control_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_message_t* msg, mavlink_change_operator_control_ack_t* change_operator_control_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + change_operator_control_ack->gcs_system_id = mavlink_msg_change_operator_control_ack_get_gcs_system_id(msg); + change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg); + change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg); +#else + memcpy(change_operator_control_ack, _MAV_PAYLOAD(msg), 3); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_command.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_command.h new file mode 100644 index 000000000..b5d44f8b2 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_command.h @@ -0,0 +1,298 @@ +// MESSAGE COMMAND PACKING + +#define MAVLINK_MSG_ID_COMMAND 75 + +typedef struct __mavlink_command_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 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) + 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. +} mavlink_command_t; + +#define MAVLINK_MSG_ID_COMMAND_LEN 20 +#define MAVLINK_MSG_ID_75_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND { \ + "COMMAND", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_command_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_command_t, target_component) }, \ + { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_t, command) }, \ + { "confirmation", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_command_t, confirmation) }, \ + { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_t, param4) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, command); + _mav_put_uint8_t(buf, 3, confirmation); + _mav_put_float(buf, 4, param1); + _mav_put_float(buf, 8, param2); + _mav_put_float(buf, 12, param3); + _mav_put_float(buf, 16, param4); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); +#else + mavlink_command_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND; + return mavlink_finalize_message(msg, system_id, component_id, 20); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, command); + _mav_put_uint8_t(buf, 3, confirmation); + _mav_put_float(buf, 4, param1); + _mav_put_float(buf, 8, param2); + _mav_put_float(buf, 12, param3); + _mav_put_float(buf, 16, param4); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); +#else + mavlink_command_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, command); + _mav_put_uint8_t(buf, 3, confirmation); + _mav_put_float(buf, 4, param1); + _mav_put_float(buf, 8, param2); + _mav_put_float(buf, 12, param3); + _mav_put_float(buf, 16, param4); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND, buf, 20); +#else + mavlink_command_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND, (const char *)&packet, 20); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 3); +} + +/** + * @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, 4); +} + +/** + * @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, 8); +} + +/** + * @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, 12); +} + +/** + * @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, 16); +} + +/** + * @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->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); + 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); +#else + memcpy(command, _MAV_PAYLOAD(msg), 20); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_command_ack.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_command_ack.h new file mode 100644 index 000000000..ee4c89dcf --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_command_ack.h @@ -0,0 +1,166 @@ +// MESSAGE COMMAND_ACK PACKING + +#define MAVLINK_MSG_ID_COMMAND_ACK 76 + +typedef struct __mavlink_command_ack_t +{ + float command; ///< Current airspeed in m/s + float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION +} mavlink_command_ack_t; + +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 +#define MAVLINK_MSG_ID_76_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ + "COMMAND_ACK", \ + 2, \ + { { "command", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ + { "result", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_ack_t, result) }, \ + } \ +} + + +/** + * @brief Pack a command_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 command Current airspeed in m/s + * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float command, float result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_float(buf, 0, command); + _mav_put_float(buf, 4, result); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; + return mavlink_finalize_message(msg, system_id, component_id, 8); +} + +/** + * @brief Pack a command_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 command Current airspeed in m/s + * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float command,float result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_float(buf, 0, command); + _mav_put_float(buf, 4, result); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); +} + +/** + * @brief Encode a command_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 command_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) +{ + return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); +} + +/** + * @brief Send a command_ack message + * @param chan MAVLink channel to send the message + * + * @param command Current airspeed in m/s + * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_float(buf, 0, command); + _mav_put_float(buf, 4, result); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, 8); +#endif +} + +#endif + +// MESSAGE COMMAND_ACK UNPACKING + + +/** + * @brief Get field command from command_ack message + * + * @return Current airspeed in m/s + */ +static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field result from command_ack message + * + * @return 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + */ +static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Decode a command_ack message into a struct + * + * @param msg The message to decode + * @param command_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP + command_ack->command = mavlink_msg_command_ack_get_command(msg); + command_ack->result = mavlink_msg_command_ack_get_result(msg); +#else + memcpy(command_ack, _MAV_PAYLOAD(msg), 8); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_control_status.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_control_status.h new file mode 100644 index 000000000..ebc1568cc --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_control_status.h @@ -0,0 +1,298 @@ +// 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_NON_CONST(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_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 8); +} + +/** + * @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_NON_CONST(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_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_debug.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_debug.h new file mode 100644 index 000000000..5a0fbdd71 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_debug.h @@ -0,0 +1,166 @@ +// MESSAGE DEBUG PACKING + +#define MAVLINK_MSG_ID_DEBUG 255 + +typedef struct __mavlink_debug_t +{ + uint8_t ind; ///< index of debug variable + float value; ///< DEBUG value +} mavlink_debug_t; + +#define MAVLINK_MSG_ID_DEBUG_LEN 5 +#define MAVLINK_MSG_ID_255_LEN 5 + + + +#define MAVLINK_MESSAGE_INFO_DEBUG { \ + "DEBUG", \ + 2, \ + { { "ind", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_debug_t, ind) }, \ + { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 1, offsetof(mavlink_debug_t, value) }, \ + } \ +} + + +/** + * @brief Pack a debug 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 ind index of debug variable + * @param value DEBUG value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[5]; + _mav_put_uint8_t(buf, 0, ind); + _mav_put_float(buf, 1, value); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); +#else + mavlink_debug_t packet; + packet.ind = ind; + packet.value = value; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG; + return mavlink_finalize_message(msg, system_id, component_id, 5); +} + +/** + * @brief Pack a debug 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 ind index of debug variable + * @param value DEBUG value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t ind,float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[5]; + _mav_put_uint8_t(buf, 0, ind); + _mav_put_float(buf, 1, value); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); +#else + mavlink_debug_t packet; + packet.ind = ind; + packet.value = value; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5); +} + +/** + * @brief Encode a debug 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 debug C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_t* debug) +{ + return mavlink_msg_debug_pack(system_id, component_id, msg, debug->ind, debug->value); +} + +/** + * @brief Send a debug message + * @param chan MAVLink channel to send the message + * + * @param ind index of debug variable + * @param value DEBUG value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[5]; + _mav_put_uint8_t(buf, 0, ind); + _mav_put_float(buf, 1, value); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, 5); +#else + mavlink_debug_t packet; + packet.ind = ind; + packet.value = value; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, 5); +#endif +} + +#endif + +// MESSAGE DEBUG UNPACKING + + +/** + * @brief Get field ind from debug message + * + * @return index of debug variable + */ +static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field value from debug message + * + * @return DEBUG value + */ +static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 1); +} + +/** + * @brief Decode a debug message into a struct + * + * @param msg The message to decode + * @param debug C-struct to decode the message contents into + */ +static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlink_debug_t* debug) +{ +#if MAVLINK_NEED_BYTE_SWAP + debug->ind = mavlink_msg_debug_get_ind(msg); + debug->value = mavlink_msg_debug_get_value(msg); +#else + memcpy(debug, _MAV_PAYLOAD(msg), 5); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_debug_vect.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_debug_vect.h new file mode 100644 index 000000000..51895f3ba --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_debug_vect.h @@ -0,0 +1,226 @@ +// MESSAGE DEBUG_VECT PACKING + +#define MAVLINK_MSG_ID_DEBUG_VECT 251 + +typedef struct __mavlink_debug_vect_t +{ + char name[10]; ///< Name + uint64_t usec; ///< Timestamp + float x; ///< x + float y; ///< y + float z; ///< z +} mavlink_debug_vect_t; + +#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 +#define MAVLINK_MSG_ID_251_LEN 30 + +#define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 + +#define MAVLINK_MESSAGE_INFO_DEBUG_VECT { \ + "DEBUG_VECT", \ + 5, \ + { { "name", NULL, MAVLINK_TYPE_CHAR, 10, 0, offsetof(mavlink_debug_vect_t, name) }, \ + { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 10, offsetof(mavlink_debug_vect_t, usec) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_debug_vect_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 22, offsetof(mavlink_debug_vect_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_debug_vect_t, z) }, \ + } \ +} + + +/** + * @brief Pack a debug_vect 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 name Name + * @param usec Timestamp + * @param x x + * @param y y + * @param z z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *name, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 10, usec); + _mav_put_float(buf, 18, x); + _mav_put_float(buf, 22, y); + _mav_put_float(buf, 26, z); + _mav_put_char_array(buf, 0, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 30); +#else + mavlink_debug_vect_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 30); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; + return mavlink_finalize_message(msg, system_id, component_id, 30); +} + +/** + * @brief Pack a debug_vect 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 name Name + * @param usec Timestamp + * @param x x + * @param y y + * @param z z + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *name,uint64_t usec,float x,float y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 10, usec); + _mav_put_float(buf, 18, x); + _mav_put_float(buf, 22, y); + _mav_put_float(buf, 26, z); + _mav_put_char_array(buf, 0, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 30); +#else + mavlink_debug_vect_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 30); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30); +} + +/** + * @brief Encode a debug_vect 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 debug_vect C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_vect_t* debug_vect) +{ + return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->usec, debug_vect->x, debug_vect->y, debug_vect->z); +} + +/** + * @brief Send a debug_vect message + * @param chan MAVLink channel to send the message + * + * @param name Name + * @param usec Timestamp + * @param x x + * @param y y + * @param z z + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char *name, uint64_t usec, float x, float y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 10, usec); + _mav_put_float(buf, 18, x); + _mav_put_float(buf, 22, y); + _mav_put_float(buf, 26, z); + _mav_put_char_array(buf, 0, name, 10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, 30); +#else + mavlink_debug_vect_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, 30); +#endif +} + +#endif + +// MESSAGE DEBUG_VECT UNPACKING + + +/** + * @brief Get field name from debug_vect message + * + * @return Name + */ +static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 0); +} + +/** + * @brief Get field usec from debug_vect message + * + * @return Timestamp + */ +static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 10); +} + +/** + * @brief Get field x from debug_vect message + * + * @return x + */ +static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 18); +} + +/** + * @brief Get field y from debug_vect message + * + * @return y + */ +static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 22); +} + +/** + * @brief Get field z from debug_vect message + * + * @return z + */ +static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 26); +} + +/** + * @brief Decode a debug_vect message into a struct + * + * @param msg The message to decode + * @param debug_vect C-struct to decode the message contents into + */ +static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, mavlink_debug_vect_t* debug_vect) +{ +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_debug_vect_get_name(msg, debug_vect->name); + debug_vect->usec = mavlink_msg_debug_vect_get_usec(msg); + debug_vect->x = mavlink_msg_debug_vect_get_x(msg); + debug_vect->y = mavlink_msg_debug_vect_get_y(msg); + debug_vect->z = mavlink_msg_debug_vect_get_z(msg); +#else + memcpy(debug_vect, _MAV_PAYLOAD(msg), 30); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_global_position.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_global_position.h new file mode 100644 index 000000000..5e0b9fe81 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_global_position.h @@ -0,0 +1,276 @@ +// 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_NON_CONST(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_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + return mavlink_finalize_message(msg, system_id, component_id, 32); +} + +/** + * @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_NON_CONST(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_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_global_position_int.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_global_position_int.h new file mode 100644 index 000000000..859a50049 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_global_position_int.h @@ -0,0 +1,254 @@ +// MESSAGE GLOBAL_POSITION_INT PACKING + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 + +typedef struct __mavlink_global_position_int_t +{ + 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 +} mavlink_global_position_int_t; + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 18 +#define MAVLINK_MSG_ID_73_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT { \ + "GLOBAL_POSITION_INT", \ + 6, \ + { { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_int_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_int_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_global_position_int_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_global_position_int_t, vz) }, \ + } \ +} + + +/** + * @brief Pack a global_position_int 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 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 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_int32_t(buf, 8, alt); + _mav_put_int16_t(buf, 12, vx); + _mav_put_int16_t(buf, 14, vy); + _mav_put_int16_t(buf, 16, vz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_global_position_int_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @brief Pack a global_position_int 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 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 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_int32_t(buf, 8, alt); + _mav_put_int16_t(buf, 12, vx); + _mav_put_int16_t(buf, 14, vy); + _mav_put_int16_t(buf, 16, vz); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_global_position_int_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); +} + +/** + * @brief Encode a global_position_int 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_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_int_t* global_position_int) +{ + return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->vx, global_position_int->vy, global_position_int->vz); +} + +/** + * @brief Send a global_position_int message + * @param chan MAVLink channel to send the message + * + * @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 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_int32_t(buf, 8, alt); + _mav_put_int16_t(buf, 12, vx); + _mav_put_int16_t(buf, 14, vy); + _mav_put_int16_t(buf, 16, vz); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, 18); +#else + mavlink_global_position_int_t packet; + 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_INT, (const char *)&packet, 18); +#endif +} + +#endif + +// MESSAGE GLOBAL_POSITION_INT UNPACKING + + +/** + * @brief Get field lat from global_position_int message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from global_position_int message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field alt from global_position_int message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field vx from global_position_int message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field vy from global_position_int message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field vz from global_position_int message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Decode a global_position_int message into a struct + * + * @param msg The message to decode + * @param global_position_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_position_int_decode(const mavlink_message_t* msg, mavlink_global_position_int_t* global_position_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_position_int->lat = mavlink_msg_global_position_int_get_lat(msg); + global_position_int->lon = mavlink_msg_global_position_int_get_lon(msg); + global_position_int->alt = mavlink_msg_global_position_int_get_alt(msg); + global_position_int->vx = mavlink_msg_global_position_int_get_vx(msg); + global_position_int->vy = mavlink_msg_global_position_int_get_vy(msg); + global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); +#else + memcpy(global_position_int, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_local_origin_set.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_local_origin_set.h new file mode 100644 index 000000000..5faec2812 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_local_origin_set.h @@ -0,0 +1,188 @@ +// 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_NON_CONST(msg), buf, 12); +#else + mavlink_gps_local_origin_set_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + return mavlink_finalize_message(msg, system_id, component_id, 12); +} + +/** + * @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_NON_CONST(msg), buf, 12); +#else + mavlink_gps_local_origin_set_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD_NON_CONST(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); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_raw.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_raw.h new file mode 100644 index 000000000..255cb27be --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_raw.h @@ -0,0 +1,320 @@ +// 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) + 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. + float lat; ///< Latitude in degrees + float lon; ///< Longitude in degrees + float alt; ///< Altitude in meters + float eph; ///< GPS HDOP + float epv; ///< GPS VDOP + float v; ///< GPS ground speed + float hdg; ///< Compass heading in degrees, 0..360 degrees +} mavlink_gps_raw_t; + +#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) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gps_raw_t, fix_type) }, \ + { "lat", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_gps_raw_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_FLOAT, 0, 13, offsetof(mavlink_gps_raw_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 17, offsetof(mavlink_gps_raw_t, alt) }, \ + { "eph", NULL, MAVLINK_TYPE_FLOAT, 0, 21, offsetof(mavlink_gps_raw_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_FLOAT, 0, 25, offsetof(mavlink_gps_raw_t, epv) }, \ + { "v", NULL, MAVLINK_TYPE_FLOAT, 0, 29, offsetof(mavlink_gps_raw_t, v) }, \ + { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 33, offsetof(mavlink_gps_raw_t, hdg) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 8, fix_type); + _mav_put_float(buf, 9, lat); + _mav_put_float(buf, 13, lon); + _mav_put_float(buf, 17, alt); + _mav_put_float(buf, 21, eph); + _mav_put_float(buf, 25, epv); + _mav_put_float(buf, 29, v); + _mav_put_float(buf, 33, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 37); +#else + mavlink_gps_raw_t packet; + packet.usec = usec; + packet.fix_type = fix_type; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 37); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; + return mavlink_finalize_message(msg, system_id, component_id, 37); +} + +/** + * @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_uint8_t(buf, 8, fix_type); + _mav_put_float(buf, 9, lat); + _mav_put_float(buf, 13, lon); + _mav_put_float(buf, 17, alt); + _mav_put_float(buf, 21, eph); + _mav_put_float(buf, 25, epv); + _mav_put_float(buf, 29, v); + _mav_put_float(buf, 33, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 37); +#else + mavlink_gps_raw_t packet; + packet.usec = usec; + packet.fix_type = fix_type; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 37); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37); +} + +/** + * @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_uint8_t(buf, 8, fix_type); + _mav_put_float(buf, 9, lat); + _mav_put_float(buf, 13, lon); + _mav_put_float(buf, 17, alt); + _mav_put_float(buf, 21, eph); + _mav_put_float(buf, 25, epv); + _mav_put_float(buf, 29, v); + _mav_put_float(buf, 33, hdg); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, buf, 37); +#else + mavlink_gps_raw_t packet; + packet.usec = usec; + packet.fix_type = fix_type; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, (const char *)&packet, 37); +#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, 8); +} + +/** + * @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, 9); +} + +/** + * @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, 13); +} + +/** + * @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, 17); +} + +/** + * @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, 21); +} + +/** + * @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, 25); +} + +/** + * @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, 29); +} + +/** + * @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, 33); +} + +/** + * @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->fix_type = mavlink_msg_gps_raw_get_fix_type(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); +#else + memcpy(gps_raw, _MAV_PAYLOAD(msg), 37); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_raw_int.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_raw_int.h new file mode 100644 index 000000000..f1e7a05e5 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_raw_int.h @@ -0,0 +1,320 @@ +// MESSAGE GPS_RAW_INT PACKING + +#define MAVLINK_MSG_ID_GPS_RAW_INT 25 + +typedef struct __mavlink_gps_raw_int_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 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. + int32_t lat; ///< Latitude in 1E7 degrees + int32_t lon; ///< Longitude in 1E7 degrees + int32_t alt; ///< Altitude in 1E3 meters (millimeters) + float eph; ///< GPS HDOP + float epv; ///< GPS VDOP + float v; ///< GPS ground speed (m/s) + float hdg; ///< Compass heading in degrees, 0..360 degrees +} mavlink_gps_raw_int_t; + +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 37 +#define MAVLINK_MSG_ID_25_LEN 37 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ + "GPS_RAW_INT", \ + 9, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, usec) }, \ + { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 9, offsetof(mavlink_gps_raw_int_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 13, offsetof(mavlink_gps_raw_int_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 17, offsetof(mavlink_gps_raw_int_t, alt) }, \ + { "eph", NULL, MAVLINK_TYPE_FLOAT, 0, 21, offsetof(mavlink_gps_raw_int_t, eph) }, \ + { "epv", NULL, MAVLINK_TYPE_FLOAT, 0, 25, offsetof(mavlink_gps_raw_int_t, epv) }, \ + { "v", NULL, MAVLINK_TYPE_FLOAT, 0, 29, offsetof(mavlink_gps_raw_int_t, v) }, \ + { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 33, offsetof(mavlink_gps_raw_int_t, hdg) }, \ + } \ +} + + +/** + * @brief Pack a gps_raw_int 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 1E7 degrees + * @param lon Longitude in 1E7 degrees + * @param alt Altitude in 1E3 meters (millimeters) + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed (m/s) + * @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_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t 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_uint8_t(buf, 8, fix_type); + _mav_put_int32_t(buf, 9, lat); + _mav_put_int32_t(buf, 13, lon); + _mav_put_int32_t(buf, 17, alt); + _mav_put_float(buf, 21, eph); + _mav_put_float(buf, 25, epv); + _mav_put_float(buf, 29, v); + _mav_put_float(buf, 33, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 37); +#else + mavlink_gps_raw_int_t packet; + packet.usec = usec; + packet.fix_type = fix_type; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 37); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + return mavlink_finalize_message(msg, system_id, component_id, 37); +} + +/** + * @brief Pack a gps_raw_int 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 1E7 degrees + * @param lon Longitude in 1E7 degrees + * @param alt Altitude in 1E3 meters (millimeters) + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed (m/s) + * @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_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t 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_uint8_t(buf, 8, fix_type); + _mav_put_int32_t(buf, 9, lat); + _mav_put_int32_t(buf, 13, lon); + _mav_put_int32_t(buf, 17, alt); + _mav_put_float(buf, 21, eph); + _mav_put_float(buf, 25, epv); + _mav_put_float(buf, 29, v); + _mav_put_float(buf, 33, hdg); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 37); +#else + mavlink_gps_raw_int_t packet; + packet.usec = usec; + packet.fix_type = fix_type; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 37); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37); +} + +/** + * @brief Encode a gps_raw_int 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_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) +{ + return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->v, gps_raw_int->hdg); +} + +/** + * @brief Send a gps_raw_int 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 1E7 degrees + * @param lon Longitude in 1E7 degrees + * @param alt Altitude in 1E3 meters (millimeters) + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed (m/s) + * @param hdg Compass heading in degrees, 0..360 degrees + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t 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_uint8_t(buf, 8, fix_type); + _mav_put_int32_t(buf, 9, lat); + _mav_put_int32_t(buf, 13, lon); + _mav_put_int32_t(buf, 17, alt); + _mav_put_float(buf, 21, eph); + _mav_put_float(buf, 25, epv); + _mav_put_float(buf, 29, v); + _mav_put_float(buf, 33, hdg); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, 37); +#else + mavlink_gps_raw_int_t packet; + packet.usec = usec; + packet.fix_type = fix_type; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, 37); +#endif +} + +#endif + +// MESSAGE GPS_RAW_INT UNPACKING + + +/** + * @brief Get field usec from gps_raw_int message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_gps_raw_int_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from gps_raw_int 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_int_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field lat from gps_raw_int message + * + * @return Latitude in 1E7 degrees + */ +static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 9); +} + +/** + * @brief Get field lon from gps_raw_int message + * + * @return Longitude in 1E7 degrees + */ +static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 13); +} + +/** + * @brief Get field alt from gps_raw_int message + * + * @return Altitude in 1E3 meters (millimeters) + */ +static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 17); +} + +/** + * @brief Get field eph from gps_raw_int message + * + * @return GPS HDOP + */ +static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 21); +} + +/** + * @brief Get field epv from gps_raw_int message + * + * @return GPS VDOP + */ +static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 25); +} + +/** + * @brief Get field v from gps_raw_int message + * + * @return GPS ground speed (m/s) + */ +static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 29); +} + +/** + * @brief Get field hdg from gps_raw_int message + * + * @return Compass heading in degrees, 0..360 degrees + */ +static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 33); +} + +/** + * @brief Decode a gps_raw_int message into a struct + * + * @param msg The message to decode + * @param gps_raw_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, mavlink_gps_raw_int_t* gps_raw_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_raw_int->usec = mavlink_msg_gps_raw_int_get_usec(msg); + gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); + gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg); + gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg); + gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg); + gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg); + gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg); + gps_raw_int->v = mavlink_msg_gps_raw_int_get_v(msg); + gps_raw_int->hdg = mavlink_msg_gps_raw_int_get_hdg(msg); +#else + memcpy(gps_raw_int, _MAV_PAYLOAD(msg), 37); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_set_global_origin.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_set_global_origin.h new file mode 100644 index 000000000..cdd1f242a --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_set_global_origin.h @@ -0,0 +1,232 @@ +// MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING + +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 + +typedef struct __mavlink_gps_set_global_origin_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + int32_t latitude; ///< global position * 1E7 + int32_t longitude; ///< global position * 1E7 + int32_t altitude; ///< global position * 1000 +} 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, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_set_global_origin_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_set_global_origin_t, target_component) }, \ + { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 2, offsetof(mavlink_gps_set_global_origin_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 6, offsetof(mavlink_gps_set_global_origin_t, longitude) }, \ + { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_gps_set_global_origin_t, altitude) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, latitude); + _mav_put_int32_t(buf, 6, longitude); + _mav_put_int32_t(buf, 10, altitude); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_gps_set_global_origin_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + return mavlink_finalize_message(msg, system_id, component_id, 14); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, latitude); + _mav_put_int32_t(buf, 6, longitude); + _mav_put_int32_t(buf, 10, altitude); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_gps_set_global_origin_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD_NON_CONST(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); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int32_t(buf, 2, latitude); + _mav_put_int32_t(buf, 6, longitude); + _mav_put_int32_t(buf, 10, altitude); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, buf, 14); +#else + mavlink_gps_set_global_origin_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, (const char *)&packet, 14); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 6); +} + +/** + * @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, 10); +} + +/** + * @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->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); + 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); +#else + memcpy(gps_set_global_origin, _MAV_PAYLOAD(msg), 14); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_status.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_status.h new file mode 100644 index 000000000..e13ffe382 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_gps_status.h @@ -0,0 +1,252 @@ +// MESSAGE GPS_STATUS PACKING + +#define MAVLINK_MSG_ID_GPS_STATUS 27 + +typedef struct __mavlink_gps_status_t +{ + uint8_t satellites_visible; ///< Number of satellites visible + int8_t satellite_prn[20]; ///< Global satellite ID + int8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization + int8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + int8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + int8_t satellite_snr[20]; ///< Signal to noise ratio of satellite +} mavlink_gps_status_t; + +#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 +#define MAVLINK_MSG_ID_27_LEN 101 + +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_USED_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_ELEVATION_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_AZIMUTH_LEN 20 +#define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_SNR_LEN 20 + +#define MAVLINK_MESSAGE_INFO_GPS_STATUS { \ + "GPS_STATUS", \ + 6, \ + { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_status_t, satellites_visible) }, \ + { "satellite_prn", NULL, MAVLINK_TYPE_INT8_T, 20, 1, offsetof(mavlink_gps_status_t, satellite_prn) }, \ + { "satellite_used", NULL, MAVLINK_TYPE_INT8_T, 20, 21, offsetof(mavlink_gps_status_t, satellite_used) }, \ + { "satellite_elevation", NULL, MAVLINK_TYPE_INT8_T, 20, 41, offsetof(mavlink_gps_status_t, satellite_elevation) }, \ + { "satellite_azimuth", NULL, MAVLINK_TYPE_INT8_T, 20, 61, offsetof(mavlink_gps_status_t, satellite_azimuth) }, \ + { "satellite_snr", NULL, MAVLINK_TYPE_INT8_T, 20, 81, offsetof(mavlink_gps_status_t, satellite_snr) }, \ + } \ +} + + +/** + * @brief Pack a gps_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 satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t satellites_visible, const int8_t *satellite_prn, const int8_t *satellite_used, const int8_t *satellite_elevation, const int8_t *satellite_azimuth, const int8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[101]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_int8_t_array(buf, 1, satellite_prn, 20); + _mav_put_int8_t_array(buf, 21, satellite_used, 20); + _mav_put_int8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_int8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_int8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(int8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 101); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 101); +} + +/** + * @brief Pack a gps_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 satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t satellites_visible,const int8_t *satellite_prn,const int8_t *satellite_used,const int8_t *satellite_elevation,const int8_t *satellite_azimuth,const int8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[101]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_int8_t_array(buf, 1, satellite_prn, 20); + _mav_put_int8_t_array(buf, 21, satellite_used, 20); + _mav_put_int8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_int8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_int8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(int8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 101); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 101); +} + +/** + * @brief Encode a gps_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 gps_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_status_t* gps_status) +{ + return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); +} + +/** + * @brief Send a gps_status message + * @param chan MAVLink channel to send the message + * + * @param satellites_visible Number of satellites visible + * @param satellite_prn Global satellite ID + * @param satellite_used 0: Satellite not used, 1: used for localization + * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite + * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. + * @param satellite_snr Signal to noise ratio of satellite + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const int8_t *satellite_prn, const int8_t *satellite_used, const int8_t *satellite_elevation, const int8_t *satellite_azimuth, const int8_t *satellite_snr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[101]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_int8_t_array(buf, 1, satellite_prn, 20); + _mav_put_int8_t_array(buf, 21, satellite_used, 20); + _mav_put_int8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_int8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_int8_t_array(buf, 81, satellite_snr, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + mav_array_memcpy(packet.satellite_prn, satellite_prn, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_used, satellite_used, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_elevation, satellite_elevation, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(int8_t)*20); + mav_array_memcpy(packet.satellite_snr, satellite_snr, sizeof(int8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, 101); +#endif +} + +#endif + +// MESSAGE GPS_STATUS UNPACKING + + +/** + * @brief Get field satellites_visible from gps_status message + * + * @return Number of satellites visible + */ +static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field satellite_prn from gps_status message + * + * @return Global satellite ID + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, int8_t *satellite_prn) +{ + return _MAV_RETURN_int8_t_array(msg, satellite_prn, 20, 1); +} + +/** + * @brief Get field satellite_used from gps_status message + * + * @return 0: Satellite not used, 1: used for localization + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, int8_t *satellite_used) +{ + return _MAV_RETURN_int8_t_array(msg, satellite_used, 20, 21); +} + +/** + * @brief Get field satellite_elevation from gps_status message + * + * @return Elevation (0: right on top of receiver, 90: on the horizon) of satellite + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, int8_t *satellite_elevation) +{ + return _MAV_RETURN_int8_t_array(msg, satellite_elevation, 20, 41); +} + +/** + * @brief Get field satellite_azimuth from gps_status message + * + * @return Direction of satellite, 0: 0 deg, 255: 360 deg. + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, int8_t *satellite_azimuth) +{ + return _MAV_RETURN_int8_t_array(msg, satellite_azimuth, 20, 61); +} + +/** + * @brief Get field satellite_snr from gps_status message + * + * @return Signal to noise ratio of satellite + */ +static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, int8_t *satellite_snr) +{ + return _MAV_RETURN_int8_t_array(msg, satellite_snr, 20, 81); +} + +/** + * @brief Decode a gps_status message into a struct + * + * @param msg The message to decode + * @param gps_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, mavlink_gps_status_t* gps_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_status->satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg); + mavlink_msg_gps_status_get_satellite_prn(msg, gps_status->satellite_prn); + mavlink_msg_gps_status_get_satellite_used(msg, gps_status->satellite_used); + mavlink_msg_gps_status_get_satellite_elevation(msg, gps_status->satellite_elevation); + mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth); + mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr); +#else + memcpy(gps_status, _MAV_PAYLOAD(msg), 101); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_heartbeat.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_heartbeat.h new file mode 100644 index 000000000..aad90d29f --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_heartbeat.h @@ -0,0 +1,185 @@ +// MESSAGE HEARTBEAT PACKING + +#define MAVLINK_MSG_ID_HEARTBEAT 0 + +typedef struct __mavlink_heartbeat_t +{ + uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + uint8_t mavlink_version; ///< MAVLink version +} mavlink_heartbeat_t; + +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_ID_0_LEN 3 + + + +#define MAVLINK_MESSAGE_INFO_HEARTBEAT { \ + "HEARTBEAT", \ + 3, \ + { { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_heartbeat_t, type) }, \ + { "autopilot", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_heartbeat_t, autopilot) }, \ + { "mavlink_version", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ + } \ +} + + +/** + * @brief Pack a heartbeat 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 type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t type, uint8_t autopilot) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, autopilot); + _mav_put_uint8_t(buf, 2, 2); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mavlink_version = 2; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + return mavlink_finalize_message(msg, system_id, component_id, 3); +} + +/** + * @brief Pack a heartbeat 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 type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t type,uint8_t autopilot) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, autopilot); + _mav_put_uint8_t(buf, 2, 2); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 3); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mavlink_version = 2; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); +} + +/** + * @brief Encode a heartbeat 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 heartbeat C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat) +{ + return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot); +} + +/** + * @brief Send a heartbeat message + * @param chan MAVLink channel to send the message + * + * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, autopilot); + _mav_put_uint8_t(buf, 2, 2); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, 3); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mavlink_version = 2; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, 3); +#endif +} + +#endif + +// MESSAGE HEARTBEAT UNPACKING + + +/** + * @brief Get field type from heartbeat message + * + * @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + */ +static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field autopilot from heartbeat message + * + * @return Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + */ +static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field mavlink_version from heartbeat message + * + * @return MAVLink version + */ +static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Decode a heartbeat message into a struct + * + * @param msg The message to decode + * @param heartbeat C-struct to decode the message contents into + */ +static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat) +{ +#if MAVLINK_NEED_BYTE_SWAP + heartbeat->type = mavlink_msg_heartbeat_get_type(msg); + heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg); + heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); +#else + memcpy(heartbeat, _MAV_PAYLOAD(msg), 3); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_hil_controls.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_hil_controls.h new file mode 100644 index 000000000..231c6f216 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_hil_controls.h @@ -0,0 +1,276 @@ +// MESSAGE HIL_CONTROLS PACKING + +#define MAVLINK_MSG_ID_HIL_CONTROLS 68 + +typedef struct __mavlink_hil_controls_t +{ + uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll_ailerons; ///< Control output -3 .. 1 + float pitch_elevator; ///< Control output -1 .. 1 + float yaw_rudder; ///< Control output -1 .. 1 + float throttle; ///< Throttle 0 .. 1 + uint8_t mode; ///< System mode (MAV_MODE) + uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE) +} mavlink_hil_controls_t; + +#define MAVLINK_MSG_ID_HIL_CONTROLS_LEN 26 +#define MAVLINK_MSG_ID_68_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_HIL_CONTROLS { \ + "HIL_CONTROLS", \ + 7, \ + { { "time_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_controls_t, time_us) }, \ + { "roll_ailerons", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_controls_t, roll_ailerons) }, \ + { "pitch_elevator", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_controls_t, pitch_elevator) }, \ + { "yaw_rudder", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_controls_t, yaw_rudder) }, \ + { "throttle", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_controls_t, throttle) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_hil_controls_t, mode) }, \ + { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_hil_controls_t, nav_mode) }, \ + } \ +} + + +/** + * @brief Pack a hil_controls 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_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -3 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, nav_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_hil_controls_t packet; + packet.time_us = time_us; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; + return mavlink_finalize_message(msg, system_id, component_id, 26); +} + +/** + * @brief Pack a hil_controls 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_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -3 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_us,float roll_ailerons,float pitch_elevator,float yaw_rudder,float throttle,uint8_t mode,uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, nav_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_hil_controls_t packet; + packet.time_us = time_us; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); +} + +/** + * @brief Encode a hil_controls 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_controls C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls) +{ + return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_us, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->mode, hil_controls->nav_mode); +} + +/** + * @brief Send a hil_controls message + * @param chan MAVLink channel to send the message + * + * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll_ailerons Control output -3 .. 1 + * @param pitch_elevator Control output -1 .. 1 + * @param yaw_rudder Control output -1 .. 1 + * @param throttle Throttle 0 .. 1 + * @param mode System mode (MAV_MODE) + * @param nav_mode Navigation mode (MAV_NAV_MODE) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_uint8_t(buf, 24, mode); + _mav_put_uint8_t(buf, 25, nav_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, 26); +#else + mavlink_hil_controls_t packet; + packet.time_us = time_us; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.mode = mode; + packet.nav_mode = nav_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, 26); +#endif +} + +#endif + +// MESSAGE HIL_CONTROLS UNPACKING + + +/** + * @brief Get field time_us from hil_controls message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll_ailerons from hil_controls message + * + * @return Control output -3 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_elevator from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_rudder from hil_controls message + * + * @return Control output -1 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field throttle from hil_controls message + * + * @return Throttle 0 .. 1 + */ +static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field mode from hil_controls message + * + * @return System mode (MAV_MODE) + */ +static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field nav_mode from hil_controls message + * + * @return Navigation mode (MAV_NAV_MODE) + */ +static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Decode a hil_controls message into a struct + * + * @param msg The message to decode + * @param hil_controls C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_controls->time_us = mavlink_msg_hil_controls_get_time_us(msg); + hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg); + hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg); + hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg); + hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg); + hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg); + hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg); +#else + memcpy(hil_controls, _MAV_PAYLOAD(msg), 26); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_hil_state.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_hil_state.h new file mode 100644 index 000000000..6692c2258 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_hil_state.h @@ -0,0 +1,474 @@ +// MESSAGE HIL_STATE PACKING + +#define MAVLINK_MSG_ID_HIL_STATE 67 + +typedef struct __mavlink_hil_state_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw 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 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) +} mavlink_hil_state_t; + +#define MAVLINK_MSG_ID_HIL_STATE_LEN 56 +#define MAVLINK_MSG_ID_67_LEN 56 + + + +#define MAVLINK_MESSAGE_INFO_HIL_STATE { \ + "HIL_STATE", \ + 16, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, usec) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \ + } \ +} + + +/** + * @brief Pack a hil_state 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 roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw 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 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_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[56]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 56); +#else + mavlink_hil_state_t packet; + packet.usec = usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + 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.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 56); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; + return mavlink_finalize_message(msg, system_id, component_id, 56); +} + +/** + * @brief Pack a hil_state 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 roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw 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 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_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,int16_t xacc,int16_t yacc,int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[56]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 56); +#else + mavlink_hil_state_t packet; + packet.usec = usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + 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.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 56); +#endif + + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 56); +} + +/** + * @brief Encode a hil_state 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 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** + * @brief Send a hil_state message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw 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 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_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[56]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, 56); +#else + mavlink_hil_state_t packet; + packet.usec = usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + 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.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, 56); +#endif +} + +#endif + +// MESSAGE HIL_STATE UNPACKING + + +/** + * @brief Get field usec from hil_state message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_hil_state_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll from hil_state message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch from hil_state message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw from hil_state message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rollspeed from hil_state message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field pitchspeed from hil_state message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yawspeed from hil_state message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field lat from hil_state message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 32); +} + +/** + * @brief Get field lon from hil_state message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field alt from hil_state message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 40); +} + +/** + * @brief Get field vx from hil_state message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 44); +} + +/** + * @brief Get field vy from hil_state message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 46); +} + +/** + * @brief Get field vz from hil_state message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 48); +} + +/** + * @brief Get field xacc from hil_state message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 50); +} + +/** + * @brief Get field yacc from hil_state message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 52); +} + +/** + * @brief Get field zacc from hil_state message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 54); +} + +/** + * @brief Decode a hil_state message into a struct + * + * @param msg The message to decode + * @param hil_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state) +{ +#if MAVLINK_NEED_BYTE_SWAP + hil_state->usec = mavlink_msg_hil_state_get_usec(msg); + hil_state->roll = mavlink_msg_hil_state_get_roll(msg); + hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg); + hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg); + hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg); + hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg); + hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg); + hil_state->lat = mavlink_msg_hil_state_get_lat(msg); + hil_state->lon = mavlink_msg_hil_state_get_lon(msg); + hil_state->alt = mavlink_msg_hil_state_get_alt(msg); + hil_state->vx = mavlink_msg_hil_state_get_vx(msg); + hil_state->vy = mavlink_msg_hil_state_get_vy(msg); + hil_state->vz = mavlink_msg_hil_state_get_vz(msg); + hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg); + hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg); + hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg); +#else + memcpy(hil_state, _MAV_PAYLOAD(msg), 56); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position.h new file mode 100644 index 000000000..121fb3af6 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position.h @@ -0,0 +1,276 @@ +// 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_NON_CONST(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_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + return mavlink_finalize_message(msg, system_id, component_id, 32); +} + +/** + * @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_NON_CONST(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_NON_CONST(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position_setpoint.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position_setpoint.h new file mode 100644 index 000000000..2e6178d96 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position_setpoint.h @@ -0,0 +1,210 @@ +// MESSAGE LOCAL_POSITION_SETPOINT PACKING + +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 + +typedef struct __mavlink_local_position_setpoint_t +{ + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< Desired yaw angle +} mavlink_local_position_setpoint_t; + +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 16 +#define MAVLINK_MSG_ID_51_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT { \ + "LOCAL_POSITION_SETPOINT", \ + 4, \ + { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_t, z) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_t, yaw) }, \ + } \ +} + + +/** + * @brief Pack a local_position_setpoint 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 Desired yaw angle + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_setpoint_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_NON_CONST(msg), buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return mavlink_finalize_message(msg, system_id, component_id, 16); +} + +/** + * @brief Pack a local_position_setpoint 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 Desired yaw angle + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_local_position_setpoint_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_NON_CONST(msg), buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); +} + +/** + * @brief Encode a local_position_setpoint 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 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_local_position_setpoint_t* local_position_setpoint) +{ + return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); +} + +/** + * @brief Send a local_position_setpoint message + * @param chan MAVLink channel to send the message + * + * @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_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_LOCAL_POSITION_SETPOINT, buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, 16); +#endif +} + +#endif + +// MESSAGE LOCAL_POSITION_SETPOINT UNPACKING + + +/** + * @brief Get field x from local_position_setpoint message + * + * @return x position + */ +static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field y from local_position_setpoint message + * + * @return y position + */ +static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field z from local_position_setpoint message + * + * @return z position + */ +static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yaw from local_position_setpoint message + * + * @return Desired yaw angle + */ +static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a local_position_setpoint message into a struct + * + * @param msg The message to decode + * @param local_position_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_message_t* msg, mavlink_local_position_setpoint_t* local_position_setpoint) +{ +#if MAVLINK_NEED_BYTE_SWAP + local_position_setpoint->x = mavlink_msg_local_position_setpoint_get_x(msg); + local_position_setpoint->y = mavlink_msg_local_position_setpoint_get_y(msg); + local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg); + local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg); +#else + memcpy(local_position_setpoint, _MAV_PAYLOAD(msg), 16); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position_setpoint_set.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position_setpoint_set.h new file mode 100644 index 000000000..e676c2832 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_local_position_setpoint_set.h @@ -0,0 +1,254 @@ +// MESSAGE LOCAL_POSITION_SETPOINT_SET PACKING + +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 + +typedef struct __mavlink_local_position_setpoint_set_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< Desired yaw angle +} 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, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_local_position_setpoint_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_local_position_setpoint_set_t, target_component) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_local_position_setpoint_set_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_local_position_setpoint_set_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_local_position_setpoint_set_t, z) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_local_position_setpoint_set_t, yaw) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, x); + _mav_put_float(buf, 6, y); + _mav_put_float(buf, 10, z); + _mav_put_float(buf, 14, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_local_position_setpoint_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, x); + _mav_put_float(buf, 6, y); + _mav_put_float(buf, 10, z); + _mav_put_float(buf, 14, yaw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_local_position_setpoint_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(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); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, x); + _mav_put_float(buf, 6, y); + _mav_put_float(buf, 10, z); + _mav_put_float(buf, 14, yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, buf, 18); +#else + mavlink_local_position_setpoint_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, (const char *)&packet, 18); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 6); +} + +/** + * @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, 10); +} + +/** + * @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, 14); +} + +/** + * @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->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); + 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); +#else + memcpy(local_position_setpoint_set, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_manual_control.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_manual_control.h new file mode 100644 index 000000000..26b70ce4a --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_manual_control.h @@ -0,0 +1,320 @@ +// MESSAGE MANUAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_MANUAL_CONTROL 69 + +typedef struct __mavlink_manual_control_t +{ + uint8_t target; ///< The system to be controlled + float roll; ///< roll + float pitch; ///< pitch + float yaw; ///< yaw + float thrust; ///< thrust + uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 + uint8_t pitch_manual; ///< pitch auto:0, manual:1 + uint8_t yaw_manual; ///< yaw auto:0, manual:1 + uint8_t thrust_manual; ///< thrust auto:0, manual:1 +} mavlink_manual_control_t; + +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 21 +#define MAVLINK_MSG_ID_69_LEN 21 + + + +#define MAVLINK_MESSAGE_INFO_MANUAL_CONTROL { \ + "MANUAL_CONTROL", \ + 9, \ + { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_manual_control_t, target) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 1, offsetof(mavlink_manual_control_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_manual_control_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_manual_control_t, yaw) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 13, offsetof(mavlink_manual_control_t, thrust) }, \ + { "roll_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_manual_control_t, roll_manual) }, \ + { "pitch_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_manual_control_t, pitch_manual) }, \ + { "yaw_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_manual_control_t, yaw_manual) }, \ + { "thrust_manual", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_manual_control_t, thrust_manual) }, \ + } \ +} + + +/** + * @brief Pack a manual_control 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 to be controlled + * @param roll roll + * @param pitch pitch + * @param yaw yaw + * @param thrust thrust + * @param roll_manual roll control enabled auto:0, manual:1 + * @param pitch_manual pitch auto:0, manual:1 + * @param yaw_manual yaw auto:0, manual:1 + * @param thrust_manual thrust auto:0, manual:1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_float(buf, 1, roll); + _mav_put_float(buf, 5, pitch); + _mav_put_float(buf, 9, yaw); + _mav_put_float(buf, 13, thrust); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21); +#else + mavlink_manual_control_t packet; + packet.target = target; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, 21); +} + +/** + * @brief Pack a manual_control 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 to be controlled + * @param roll roll + * @param pitch pitch + * @param yaw yaw + * @param thrust thrust + * @param roll_manual roll control enabled auto:0, manual:1 + * @param pitch_manual pitch auto:0, manual:1 + * @param yaw_manual yaw auto:0, manual:1 + * @param thrust_manual thrust auto:0, manual:1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,float roll,float pitch,float yaw,float thrust,uint8_t roll_manual,uint8_t pitch_manual,uint8_t yaw_manual,uint8_t thrust_manual) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_float(buf, 1, roll); + _mav_put_float(buf, 5, pitch); + _mav_put_float(buf, 9, yaw); + _mav_put_float(buf, 13, thrust); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21); +#else + mavlink_manual_control_t packet; + packet.target = target; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21); +#endif + + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21); +} + +/** + * @brief Encode a manual_control 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 manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_manual_control_t* manual_control) +{ + return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->roll, manual_control->pitch, manual_control->yaw, manual_control->thrust, manual_control->roll_manual, manual_control->pitch_manual, manual_control->yaw_manual, manual_control->thrust_manual); +} + +/** + * @brief Send a manual_control message + * @param chan MAVLink channel to send the message + * + * @param target The system to be controlled + * @param roll roll + * @param pitch pitch + * @param yaw yaw + * @param thrust thrust + * @param roll_manual roll control enabled auto:0, manual:1 + * @param pitch_manual pitch auto:0, manual:1 + * @param yaw_manual yaw auto:0, manual:1 + * @param thrust_manual thrust auto:0, manual:1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_float(buf, 1, roll); + _mav_put_float(buf, 5, pitch); + _mav_put_float(buf, 9, yaw); + _mav_put_float(buf, 13, thrust); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, 21); +#else + mavlink_manual_control_t packet; + packet.target = target; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, 21); +#endif +} + +#endif + +// MESSAGE MANUAL_CONTROL UNPACKING + + +/** + * @brief Get field target from manual_control message + * + * @return The system to be controlled + */ +static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field roll from manual_control message + * + * @return roll + */ +static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 1); +} + +/** + * @brief Get field pitch from manual_control message + * + * @return pitch + */ +static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 5); +} + +/** + * @brief Get field yaw from manual_control message + * + * @return yaw + */ +static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 9); +} + +/** + * @brief Get field thrust from manual_control message + * + * @return thrust + */ +static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 13); +} + +/** + * @brief Get field roll_manual from manual_control message + * + * @return roll control enabled auto:0, manual:1 + */ +static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field pitch_manual from manual_control message + * + * @return pitch auto:0, manual:1 + */ +static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field yaw_manual from manual_control message + * + * @return yaw auto:0, manual:1 + */ +static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field thrust_manual from manual_control message + * + * @return thrust auto:0, manual:1 + */ +static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Decode a manual_control message into a struct + * + * @param msg The message to decode + * @param manual_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* msg, mavlink_manual_control_t* manual_control) +{ +#if MAVLINK_NEED_BYTE_SWAP + manual_control->target = mavlink_msg_manual_control_get_target(msg); + manual_control->roll = mavlink_msg_manual_control_get_roll(msg); + manual_control->pitch = mavlink_msg_manual_control_get_pitch(msg); + manual_control->yaw = mavlink_msg_manual_control_get_yaw(msg); + manual_control->thrust = mavlink_msg_manual_control_get_thrust(msg); + manual_control->roll_manual = mavlink_msg_manual_control_get_roll_manual(msg); + manual_control->pitch_manual = mavlink_msg_manual_control_get_pitch_manual(msg); + manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg); + manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg); +#else + memcpy(manual_control, _MAV_PAYLOAD(msg), 21); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_named_value_float.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_named_value_float.h new file mode 100644 index 000000000..5cf76e372 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_named_value_float.h @@ -0,0 +1,160 @@ +// MESSAGE NAMED_VALUE_FLOAT PACKING + +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 252 + +typedef struct __mavlink_named_value_float_t +{ + char name[10]; ///< Name of the debug variable + float value; ///< Floating point value +} mavlink_named_value_float_t; + +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 14 +#define MAVLINK_MSG_ID_252_LEN 14 + +#define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 + +#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT { \ + "NAMED_VALUE_FLOAT", \ + 2, \ + { { "name", NULL, MAVLINK_TYPE_CHAR, 10, 0, offsetof(mavlink_named_value_float_t, name) }, \ + { "value", NULL, MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_named_value_float_t, value) }, \ + } \ +} + + +/** + * @brief Pack a named_value_float 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 name Name of the debug variable + * @param value Floating point value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_float(buf, 10, value); + _mav_put_char_array(buf, 0, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_named_value_float_t packet; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + return mavlink_finalize_message(msg, system_id, component_id, 14); +} + +/** + * @brief Pack a named_value_float 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 name Name of the debug variable + * @param value Floating point value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *name,float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_float(buf, 10, value); + _mav_put_char_array(buf, 0, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_named_value_float_t packet; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); +} + +/** + * @brief Encode a named_value_float 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 named_value_float C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_float_t* named_value_float) +{ + return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->name, named_value_float->value); +} + +/** + * @brief Send a named_value_float message + * @param chan MAVLink channel to send the message + * + * @param name Name of the debug variable + * @param value Floating point value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char *name, float value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_float(buf, 10, value); + _mav_put_char_array(buf, 0, name, 10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, 14); +#else + mavlink_named_value_float_t packet; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, 14); +#endif +} + +#endif + +// MESSAGE NAMED_VALUE_FLOAT UNPACKING + + +/** + * @brief Get field name from named_value_float message + * + * @return Name of the debug variable + */ +static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 0); +} + +/** + * @brief Get field value from named_value_float message + * + * @return Floating point value + */ +static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 10); +} + +/** + * @brief Decode a named_value_float message into a struct + * + * @param msg The message to decode + * @param named_value_float C-struct to decode the message contents into + */ +static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* msg, mavlink_named_value_float_t* named_value_float) +{ +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_named_value_float_get_name(msg, named_value_float->name); + named_value_float->value = mavlink_msg_named_value_float_get_value(msg); +#else + memcpy(named_value_float, _MAV_PAYLOAD(msg), 14); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_named_value_int.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_named_value_int.h new file mode 100644 index 000000000..93f0911aa --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_named_value_int.h @@ -0,0 +1,160 @@ +// MESSAGE NAMED_VALUE_INT PACKING + +#define MAVLINK_MSG_ID_NAMED_VALUE_INT 253 + +typedef struct __mavlink_named_value_int_t +{ + char name[10]; ///< Name of the debug variable + int32_t value; ///< Signed integer value +} mavlink_named_value_int_t; + +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 14 +#define MAVLINK_MSG_ID_253_LEN 14 + +#define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 + +#define MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT { \ + "NAMED_VALUE_INT", \ + 2, \ + { { "name", NULL, MAVLINK_TYPE_CHAR, 10, 0, offsetof(mavlink_named_value_int_t, name) }, \ + { "value", NULL, MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_named_value_int_t, value) }, \ + } \ +} + + +/** + * @brief Pack a named_value_int 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 name Name of the debug variable + * @param value Signed integer value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 10, value); + _mav_put_char_array(buf, 0, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_named_value_int_t packet; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + return mavlink_finalize_message(msg, system_id, component_id, 14); +} + +/** + * @brief Pack a named_value_int 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 name Name of the debug variable + * @param value Signed integer value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *name,int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 10, value); + _mav_put_char_array(buf, 0, name, 10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_named_value_int_t packet; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); +} + +/** + * @brief Encode a named_value_int 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 named_value_int C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_named_value_int_t* named_value_int) +{ + return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->name, named_value_int->value); +} + +/** + * @brief Send a named_value_int message + * @param chan MAVLink channel to send the message + * + * @param name Name of the debug variable + * @param value Signed integer value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char *name, int32_t value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 10, value); + _mav_put_char_array(buf, 0, name, 10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, 14); +#else + mavlink_named_value_int_t packet; + packet.value = value; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, 14); +#endif +} + +#endif + +// MESSAGE NAMED_VALUE_INT UNPACKING + + +/** + * @brief Get field name from named_value_int message + * + * @return Name of the debug variable + */ +static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 0); +} + +/** + * @brief Get field value from named_value_int message + * + * @return Signed integer value + */ +static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 10); +} + +/** + * @brief Decode a named_value_int message into a struct + * + * @param msg The message to decode + * @param named_value_int C-struct to decode the message contents into + */ +static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* msg, mavlink_named_value_int_t* named_value_int) +{ +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_named_value_int_get_name(msg, named_value_int->name); + named_value_int->value = mavlink_msg_named_value_int_get_value(msg); +#else + memcpy(named_value_int, _MAV_PAYLOAD(msg), 14); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_nav_controller_output.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_nav_controller_output.h new file mode 100644 index 000000000..64a4a7fb4 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_nav_controller_output.h @@ -0,0 +1,298 @@ +// MESSAGE NAV_CONTROLLER_OUTPUT PACKING + +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 + +typedef struct __mavlink_nav_controller_output_t +{ + float nav_roll; ///< Current desired roll in degrees + float nav_pitch; ///< Current desired pitch in degrees + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current waypoint/target in degrees + uint16_t wp_dist; ///< Distance to active waypoint in meters + float alt_error; ///< Current altitude error in meters + float aspd_error; ///< Current airspeed error in meters/second + float xtrack_error; ///< Current crosstrack error on x-y plane in meters +} mavlink_nav_controller_output_t; + +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 +#define MAVLINK_MSG_ID_62_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT { \ + "NAV_CONTROLLER_OUTPUT", \ + 8, \ + { { "nav_roll", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_nav_controller_output_t, nav_roll) }, \ + { "nav_pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_nav_controller_output_t, nav_pitch) }, \ + { "nav_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_nav_controller_output_t, nav_bearing) }, \ + { "target_bearing", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_nav_controller_output_t, target_bearing) }, \ + { "wp_dist", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_nav_controller_output_t, wp_dist) }, \ + { "alt_error", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_nav_controller_output_t, alt_error) }, \ + { "aspd_error", NULL, MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_nav_controller_output_t, aspd_error) }, \ + { "xtrack_error", NULL, MAVLINK_TYPE_FLOAT, 0, 22, offsetof(mavlink_nav_controller_output_t, xtrack_error) }, \ + } \ +} + + +/** + * @brief Pack a nav_controller_output message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_int16_t(buf, 8, nav_bearing); + _mav_put_int16_t(buf, 10, target_bearing); + _mav_put_uint16_t(buf, 12, wp_dist); + _mav_put_float(buf, 14, alt_error); + _mav_put_float(buf, 18, aspd_error); + _mav_put_float(buf, 22, xtrack_error); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + return mavlink_finalize_message(msg, system_id, component_id, 26); +} + +/** + * @brief Pack a nav_controller_output 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 nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float nav_roll,float nav_pitch,int16_t nav_bearing,int16_t target_bearing,uint16_t wp_dist,float alt_error,float aspd_error,float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_int16_t(buf, 8, nav_bearing); + _mav_put_int16_t(buf, 10, target_bearing); + _mav_put_uint16_t(buf, 12, wp_dist); + _mav_put_float(buf, 14, alt_error); + _mav_put_float(buf, 18, aspd_error); + _mav_put_float(buf, 22, xtrack_error); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); +} + +/** + * @brief Encode a nav_controller_output struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param nav_controller_output C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_nav_controller_output_t* nav_controller_output) +{ + return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); +} + +/** + * @brief Send a nav_controller_output message + * @param chan MAVLink channel to send the message + * + * @param nav_roll Current desired roll in degrees + * @param nav_pitch Current desired pitch in degrees + * @param nav_bearing Current desired heading in degrees + * @param target_bearing Bearing to current waypoint/target in degrees + * @param wp_dist Distance to active waypoint in meters + * @param alt_error Current altitude error in meters + * @param aspd_error Current airspeed error in meters/second + * @param xtrack_error Current crosstrack error on x-y plane in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_int16_t(buf, 8, nav_bearing); + _mav_put_int16_t(buf, 10, target_bearing); + _mav_put_uint16_t(buf, 12, wp_dist); + _mav_put_float(buf, 14, alt_error); + _mav_put_float(buf, 18, aspd_error); + _mav_put_float(buf, 22, xtrack_error); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, 26); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, 26); +#endif +} + +#endif + +// MESSAGE NAV_CONTROLLER_OUTPUT UNPACKING + + +/** + * @brief Get field nav_roll from nav_controller_output message + * + * @return Current desired roll in degrees + */ +static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field nav_pitch from nav_controller_output message + * + * @return Current desired pitch in degrees + */ +static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field nav_bearing from nav_controller_output message + * + * @return Current desired heading in degrees + */ +static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field target_bearing from nav_controller_output message + * + * @return Bearing to current waypoint/target in degrees + */ +static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field wp_dist from nav_controller_output message + * + * @return Distance to active waypoint in meters + */ +static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field alt_error from nav_controller_output message + * + * @return Current altitude error in meters + */ +static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 14); +} + +/** + * @brief Get field aspd_error from nav_controller_output message + * + * @return Current airspeed error in meters/second + */ +static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 18); +} + +/** + * @brief Get field xtrack_error from nav_controller_output message + * + * @return Current crosstrack error on x-y plane in meters + */ +static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 22); +} + +/** + * @brief Decode a nav_controller_output message into a struct + * + * @param msg The message to decode + * @param nav_controller_output C-struct to decode the message contents into + */ +static inline void mavlink_msg_nav_controller_output_decode(const mavlink_message_t* msg, mavlink_nav_controller_output_t* nav_controller_output) +{ +#if MAVLINK_NEED_BYTE_SWAP + nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); + nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); + nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); + nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); + nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); + nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); + nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); + nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); +#else + memcpy(nav_controller_output, _MAV_PAYLOAD(msg), 26); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_object_detection_event.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_object_detection_event.h new file mode 100644 index 000000000..617eb50d4 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_object_detection_event.h @@ -0,0 +1,270 @@ +// 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 + 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 + 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 +} 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) }, \ + { "object_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_object_detection_event_t, object_id) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_object_detection_event_t, type) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 20, 7, offsetof(mavlink_object_detection_event_t, name) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_object_detection_event_t, quality) }, \ + { "bearing", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_object_detection_event_t, bearing) }, \ + { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_object_detection_event_t, distance) }, \ + } \ +} + + +/** + * @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_uint16_t(buf, 4, object_id); + _mav_put_uint8_t(buf, 6, type); + _mav_put_uint8_t(buf, 27, quality); + _mav_put_float(buf, 28, bearing); + _mav_put_float(buf, 32, distance); + _mav_put_char_array(buf, 7, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36); +#else + mavlink_object_detection_event_t packet; + packet.time = time; + packet.object_id = object_id; + packet.type = type; + packet.quality = quality; + packet.bearing = bearing; + packet.distance = distance; + mav_array_memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + return mavlink_finalize_message(msg, system_id, component_id, 36); +} + +/** + * @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_uint16_t(buf, 4, object_id); + _mav_put_uint8_t(buf, 6, type); + _mav_put_uint8_t(buf, 27, quality); + _mav_put_float(buf, 28, bearing); + _mav_put_float(buf, 32, distance); + _mav_put_char_array(buf, 7, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36); +#else + mavlink_object_detection_event_t packet; + packet.time = time; + packet.object_id = object_id; + packet.type = type; + packet.quality = quality; + packet.bearing = bearing; + packet.distance = distance; + mav_array_memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36); +} + +/** + * @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_uint16_t(buf, 4, object_id); + _mav_put_uint8_t(buf, 6, type); + _mav_put_uint8_t(buf, 27, quality); + _mav_put_float(buf, 28, bearing); + _mav_put_float(buf, 32, distance); + _mav_put_char_array(buf, 7, name, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT, buf, 36); +#else + mavlink_object_detection_event_t packet; + packet.time = time; + packet.object_id = object_id; + packet.type = type; + packet.quality = quality; + packet.bearing = bearing; + packet.distance = distance; + 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); +#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, 4); +} + +/** + * @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, 6); +} + +/** + * @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, 7); +} + +/** + * @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, 27); +} + +/** + * @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, 28); +} + +/** + * @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, 32); +} + +/** + * @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->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); + object_detection_event->bearing = mavlink_msg_object_detection_event_get_bearing(msg); + object_detection_event->distance = mavlink_msg_object_detection_event_get_distance(msg); +#else + memcpy(object_detection_event, _MAV_PAYLOAD(msg), 36); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_optical_flow.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_optical_flow.h new file mode 100644 index 000000000..33067cc89 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_optical_flow.h @@ -0,0 +1,254 @@ +// MESSAGE OPTICAL_FLOW PACKING + +#define MAVLINK_MSG_ID_OPTICAL_FLOW 100 + +typedef struct __mavlink_optical_flow_t +{ + uint64_t time; ///< Timestamp (UNIX) + uint8_t sensor_id; ///< Sensor ID + int16_t flow_x; ///< Flow in pixels in x-sensor direction + int16_t flow_y; ///< Flow in pixels in y-sensor direction + uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality + float ground_distance; ///< Ground distance in meters +} mavlink_optical_flow_t; + +#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 18 +#define MAVLINK_MSG_ID_100_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ + "OPTICAL_FLOW", \ + 6, \ + { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time) }, \ + { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_optical_flow_t, sensor_id) }, \ + { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 9, offsetof(mavlink_optical_flow_t, flow_x) }, \ + { "flow_y", NULL, MAVLINK_TYPE_INT16_T, 0, 11, offsetof(mavlink_optical_flow_t, flow_y) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_optical_flow_t, quality) }, \ + { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + } \ +} + + +/** + * @brief Pack a 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 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 quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_uint8_t(buf, 8, sensor_id); + _mav_put_int16_t(buf, 9, flow_x); + _mav_put_int16_t(buf, 11, flow_y); + _mav_put_uint8_t(buf, 13, quality); + _mav_put_float(buf, 14, ground_distance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_optical_flow_t packet; + packet.time = time; + packet.sensor_id = sensor_id; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.quality = quality; + packet.ground_distance = ground_distance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @brief Pack a 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 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 quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_uint8_t(buf, 8, sensor_id); + _mav_put_int16_t(buf, 9, flow_x); + _mav_put_int16_t(buf, 11, flow_y); + _mav_put_uint8_t(buf, 13, quality); + _mav_put_float(buf, 14, ground_distance); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_optical_flow_t packet; + packet.time = time; + packet.sensor_id = sensor_id; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.quality = quality; + packet.ground_distance = ground_distance; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); +} + +/** + * @brief Encode a 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 optical_flow C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) +{ + return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->quality, optical_flow->ground_distance); +} + +/** + * @brief Send a optical_flow message + * @param chan MAVLink channel to send the message + * + * @param time 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 quality Optical flow quality / confidence. 0: bad, 255: maximum quality + * @param ground_distance Ground distance in meters + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_uint8_t(buf, 8, sensor_id); + _mav_put_int16_t(buf, 9, flow_x); + _mav_put_int16_t(buf, 11, flow_y); + _mav_put_uint8_t(buf, 13, quality); + _mav_put_float(buf, 14, ground_distance); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, 18); +#else + mavlink_optical_flow_t packet; + packet.time = time; + packet.sensor_id = sensor_id; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.quality = quality; + packet.ground_distance = ground_distance; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, 18); +#endif +} + +#endif + +// MESSAGE OPTICAL_FLOW UNPACKING + + +/** + * @brief Get field time from optical_flow message + * + * @return Timestamp (UNIX) + */ +static inline uint64_t mavlink_msg_optical_flow_get_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_id from optical_flow message + * + * @return Sensor ID + */ +static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field flow_x from optical_flow message + * + * @return Flow in pixels in x-sensor direction + */ +static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 9); +} + +/** + * @brief Get field flow_y from optical_flow message + * + * @return Flow in pixels in y-sensor direction + */ +static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 11); +} + +/** + * @brief Get field quality from optical_flow message + * + * @return Optical flow quality / confidence. 0: bad, 255: maximum quality + */ +static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field ground_distance from optical_flow message + * + * @return Ground distance in meters + */ +static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 14); +} + +/** + * @brief Decode a optical_flow message into a struct + * + * @param msg The message to decode + * @param optical_flow C-struct to decode the message contents into + */ +static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow) +{ +#if MAVLINK_NEED_BYTE_SWAP + optical_flow->time = mavlink_msg_optical_flow_get_time(msg); + optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); + optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg); + optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); + optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); + optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg); +#else + memcpy(optical_flow, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_request_list.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_request_list.h new file mode 100644 index 000000000..39e35915e --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_request_list.h @@ -0,0 +1,166 @@ +// MESSAGE PARAM_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 + +typedef struct __mavlink_param_request_list_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_param_request_list_t; + +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_21_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST { \ + "PARAM_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a param_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_param_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_NON_CONST(msg), buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @brief Pack a param_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_param_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_NON_CONST(msg), buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @brief Encode a param_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 param_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_list_t* param_request_list) +{ + return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); +} + +/** + * @brief Send a param_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_param_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_PARAM_REQUEST_LIST, buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, 2); +#endif +} + +#endif + +// MESSAGE PARAM_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from param_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a param_request_list message into a struct + * + * @param msg The message to decode + * @param param_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t* msg, mavlink_param_request_list_t* param_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); + param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); +#else + memcpy(param_request_list, _MAV_PAYLOAD(msg), 2); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_request_read.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_request_read.h new file mode 100644 index 000000000..c02cb0449 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_request_read.h @@ -0,0 +1,204 @@ +// MESSAGE PARAM_REQUEST_READ PACKING + +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 + +typedef struct __mavlink_param_request_read_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + int8_t param_id[15]; ///< Onboard parameter id + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier +} mavlink_param_request_read_t; + +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 19 +#define MAVLINK_MSG_ID_20_LEN 19 + +#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15 + +#define MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ { \ + "PARAM_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_INT8_T, 15, 2, offsetof(mavlink_param_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 17, offsetof(mavlink_param_request_read_t, param_index) }, \ + } \ +} + + +/** + * @brief Pack a param_request_read 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 param_id Onboard parameter id + * @param param_index Parameter index. Send -1 to use the param ID field as identifier + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const int8_t *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[19]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int16_t(buf, 17, param_index); + _mav_put_int8_t_array(buf, 2, param_id, 15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 19); +#else + mavlink_param_request_read_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_index = param_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 19); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + return mavlink_finalize_message(msg, system_id, component_id, 19); +} + +/** + * @brief Pack a param_request_read 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 param_id Onboard parameter id + * @param param_index Parameter index. Send -1 to use the param ID field as identifier + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_request_read_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,const int8_t *param_id,int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[19]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int16_t(buf, 17, param_index); + _mav_put_int8_t_array(buf, 2, param_id, 15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 19); +#else + mavlink_param_request_read_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_index = param_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 19); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 19); +} + +/** + * @brief Encode a param_request_read 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 param_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_request_read_t* param_request_read) +{ + return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); +} + +/** + * @brief Send a param_request_read message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id + * @param param_index Parameter index. Send -1 to use the param ID field as identifier + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[19]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int16_t(buf, 17, param_index); + _mav_put_int8_t_array(buf, 2, param_id, 15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, 19); +#else + mavlink_param_request_read_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_index = param_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, 19); +#endif +} + +#endif + +// MESSAGE PARAM_REQUEST_READ UNPACKING + + +/** + * @brief Get field target_system from param_request_read message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_request_read message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field param_id from param_request_read message + * + * @return Onboard parameter id + */ +static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, int8_t *param_id) +{ + return _MAV_RETURN_int8_t_array(msg, param_id, 15, 2); +} + +/** + * @brief Get field param_index from param_request_read message + * + * @return Parameter index. Send -1 to use the param ID field as identifier + */ +static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 17); +} + +/** + * @brief Decode a param_request_read message into a struct + * + * @param msg The message to decode + * @param param_request_read C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t* msg, mavlink_param_request_read_t* param_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_request_read->target_system = mavlink_msg_param_request_read_get_target_system(msg); + param_request_read->target_component = mavlink_msg_param_request_read_get_target_component(msg); + mavlink_msg_param_request_read_get_param_id(msg, param_request_read->param_id); + param_request_read->param_index = mavlink_msg_param_request_read_get_param_index(msg); +#else + memcpy(param_request_read, _MAV_PAYLOAD(msg), 19); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_set.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_set.h new file mode 100644 index 000000000..e6648430f --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_set.h @@ -0,0 +1,204 @@ +// MESSAGE PARAM_SET PACKING + +#define MAVLINK_MSG_ID_PARAM_SET 23 + +typedef struct __mavlink_param_set_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + int8_t param_id[15]; ///< Onboard parameter id + float param_value; ///< Onboard parameter value +} mavlink_param_set_t; + +#define MAVLINK_MSG_ID_PARAM_SET_LEN 21 +#define MAVLINK_MSG_ID_23_LEN 21 + +#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15 + +#define MAVLINK_MESSAGE_INFO_PARAM_SET { \ + "PARAM_SET", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_INT8_T, 15, 2, offsetof(mavlink_param_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 17, offsetof(mavlink_param_set_t, param_value) }, \ + } \ +} + + +/** + * @brief Pack a param_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 param_id Onboard parameter id + * @param param_value Onboard parameter value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const int8_t *param_id, float param_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 17, param_value); + _mav_put_int8_t_array(buf, 2, param_id, 15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21); +#else + mavlink_param_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_value = param_value; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; + return mavlink_finalize_message(msg, system_id, component_id, 21); +} + +/** + * @brief Pack a param_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 param_id Onboard parameter id + * @param param_value Onboard parameter value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_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,const int8_t *param_id,float param_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 17, param_value); + _mav_put_int8_t_array(buf, 2, param_id, 15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 21); +#else + mavlink_param_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_value = param_value; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 21); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21); +} + +/** + * @brief Encode a param_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 param_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_set_t* param_set) +{ + return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value); +} + +/** + * @brief Send a param_set message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Onboard parameter id + * @param param_value Onboard parameter value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t *param_id, float param_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 17, param_value); + _mav_put_int8_t_array(buf, 2, param_id, 15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, 21); +#else + mavlink_param_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_value = param_value; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, 21); +#endif +} + +#endif + +// MESSAGE PARAM_SET UNPACKING + + +/** + * @brief Get field target_system from param_set message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_set message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field param_id from param_set message + * + * @return Onboard parameter id + */ +static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, int8_t *param_id) +{ + return _MAV_RETURN_int8_t_array(msg, param_id, 15, 2); +} + +/** + * @brief Get field param_value from param_set message + * + * @return Onboard parameter value + */ +static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 17); +} + +/** + * @brief Decode a param_set message into a struct + * + * @param msg The message to decode + * @param param_set C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, mavlink_param_set_t* param_set) +{ +#if MAVLINK_NEED_BYTE_SWAP + param_set->target_system = mavlink_msg_param_set_get_target_system(msg); + param_set->target_component = mavlink_msg_param_set_get_target_component(msg); + mavlink_msg_param_set_get_param_id(msg, param_set->param_id); + param_set->param_value = mavlink_msg_param_set_get_param_value(msg); +#else + memcpy(param_set, _MAV_PAYLOAD(msg), 21); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_value.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_value.h new file mode 100644 index 000000000..634fbb152 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_param_value.h @@ -0,0 +1,204 @@ +// MESSAGE PARAM_VALUE PACKING + +#define MAVLINK_MSG_ID_PARAM_VALUE 22 + +typedef struct __mavlink_param_value_t +{ + int8_t param_id[15]; ///< Onboard parameter id + float param_value; ///< Onboard parameter value + uint16_t param_count; ///< Total number of onboard parameters + uint16_t param_index; ///< Index of this onboard parameter +} mavlink_param_value_t; + +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 23 +#define MAVLINK_MSG_ID_22_LEN 23 + +#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 15 + +#define MAVLINK_MESSAGE_INFO_PARAM_VALUE { \ + "PARAM_VALUE", \ + 4, \ + { { "param_id", NULL, MAVLINK_TYPE_INT8_T, 15, 0, offsetof(mavlink_param_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_FLOAT, 0, 15, offsetof(mavlink_param_value_t, param_value) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 19, offsetof(mavlink_param_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_param_value_t, param_index) }, \ + } \ +} + + +/** + * @brief Pack a param_value 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 param_id Onboard parameter id + * @param param_value Onboard parameter value + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const int8_t *param_id, float param_value, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[23]; + _mav_put_float(buf, 15, param_value); + _mav_put_uint16_t(buf, 19, param_count); + _mav_put_uint16_t(buf, 21, param_index); + _mav_put_int8_t_array(buf, 0, param_id, 15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 23); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 23); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; + return mavlink_finalize_message(msg, system_id, component_id, 23); +} + +/** + * @brief Pack a param_value 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 param_id Onboard parameter id + * @param param_value Onboard parameter value + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const int8_t *param_id,float param_value,uint16_t param_count,uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[23]; + _mav_put_float(buf, 15, param_value); + _mav_put_uint16_t(buf, 19, param_count); + _mav_put_uint16_t(buf, 21, param_index); + _mav_put_int8_t_array(buf, 0, param_id, 15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 23); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 23); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23); +} + +/** + * @brief Encode a param_value 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 param_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_value_t* param_value) +{ + return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_count, param_value->param_index); +} + +/** + * @brief Send a param_value message + * @param chan MAVLink channel to send the message + * + * @param param_id Onboard parameter id + * @param param_value Onboard parameter value + * @param param_count Total number of onboard parameters + * @param param_index Index of this onboard parameter + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const int8_t *param_id, float param_value, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[23]; + _mav_put_float(buf, 15, param_value); + _mav_put_uint16_t(buf, 19, param_count); + _mav_put_uint16_t(buf, 21, param_index); + _mav_put_int8_t_array(buf, 0, param_id, 15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, 23); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + mav_array_memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, 23); +#endif +} + +#endif + +// MESSAGE PARAM_VALUE UNPACKING + + +/** + * @brief Get field param_id from param_value message + * + * @return Onboard parameter id + */ +static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, int8_t *param_id) +{ + return _MAV_RETURN_int8_t_array(msg, param_id, 15, 0); +} + +/** + * @brief Get field param_value from param_value message + * + * @return Onboard parameter value + */ +static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 15); +} + +/** + * @brief Get field param_count from param_value message + * + * @return Total number of onboard parameters + */ +static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 19); +} + +/** + * @brief Get field param_index from param_value message + * + * @return Index of this onboard parameter + */ +static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 21); +} + +/** + * @brief Decode a param_value message into a struct + * + * @param msg The message to decode + * @param param_value C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value) +{ +#if MAVLINK_NEED_BYTE_SWAP + mavlink_msg_param_value_get_param_id(msg, param_value->param_id); + param_value->param_value = mavlink_msg_param_value_get_param_value(msg); + param_value->param_count = mavlink_msg_param_value_get_param_count(msg); + param_value->param_index = mavlink_msg_param_value_get_param_index(msg); +#else + memcpy(param_value, _MAV_PAYLOAD(msg), 23); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_ping.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_ping.h new file mode 100644 index 000000000..ad076fd9b --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_ping.h @@ -0,0 +1,210 @@ +// MESSAGE PING PACKING + +#define MAVLINK_MSG_ID_PING 3 + +typedef struct __mavlink_ping_t +{ + uint32_t seq; ///< PING sequence + uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + uint64_t time; ///< Unix timestamp in microseconds +} mavlink_ping_t; + +#define MAVLINK_MSG_ID_PING_LEN 14 +#define MAVLINK_MSG_ID_3_LEN 14 + + + +#define MAVLINK_MESSAGE_INFO_PING { \ + "PING", \ + 4, \ + { { "seq", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ping_t, seq) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_ping_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_ping_t, target_component) }, \ + { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 6, offsetof(mavlink_ping_t, time) }, \ + } \ +} + + +/** + * @brief Pack a ping 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 PING sequence + * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param time Unix timestamp in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint32_t(buf, 0, seq); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint64_t(buf, 6, time); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_ping_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.time = time; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_PING; + return mavlink_finalize_message(msg, system_id, component_id, 14); +} + +/** + * @brief Pack a ping 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 PING sequence + * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param time Unix timestamp in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t seq,uint8_t target_system,uint8_t target_component,uint64_t time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint32_t(buf, 0, seq); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint64_t(buf, 6, time); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 14); +#else + mavlink_ping_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.time = time; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_PING; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); +} + +/** + * @brief Encode a ping 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 ping C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ping_t* ping) +{ + return mavlink_msg_ping_pack(system_id, component_id, msg, ping->seq, ping->target_system, ping->target_component, ping->time); +} + +/** + * @brief Send a ping message + * @param chan MAVLink channel to send the message + * + * @param seq PING sequence + * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + * @param time Unix timestamp in microseconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint32_t(buf, 0, seq); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint64_t(buf, 6, time); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, 14); +#else + mavlink_ping_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.time = time; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, 14); +#endif +} + +#endif + +// MESSAGE PING UNPACKING + + +/** + * @brief Get field seq from ping message + * + * @return PING sequence + */ +static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field target_system from ping message + * + * @return 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + */ +static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from ping message + * + * @return 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + */ +static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field time from ping message + * + * @return Unix timestamp in microseconds + */ +static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 6); +} + +/** + * @brief Decode a ping message into a struct + * + * @param msg The message to decode + * @param ping C-struct to decode the message contents into + */ +static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink_ping_t* ping) +{ +#if MAVLINK_NEED_BYTE_SWAP + ping->seq = mavlink_msg_ping_get_seq(msg); + ping->target_system = mavlink_msg_ping_get_target_system(msg); + ping->target_component = mavlink_msg_ping_get_target_component(msg); + ping->time = mavlink_msg_ping_get_time(msg); +#else + memcpy(ping, _MAV_PAYLOAD(msg), 14); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_position_target.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_position_target.h new file mode 100644 index 000000000..6cd7719fb --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_position_target.h @@ -0,0 +1,210 @@ +// 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_NON_CONST(msg), buf, 16); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; + return mavlink_finalize_message(msg, system_id, component_id, 16); +} + +/** + * @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_NON_CONST(msg), buf, 16); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_raw_imu.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_raw_imu.h new file mode 100644 index 000000000..c60945ae9 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_raw_imu.h @@ -0,0 +1,342 @@ +// MESSAGE RAW_IMU PACKING + +#define MAVLINK_MSG_ID_RAW_IMU 28 + +typedef struct __mavlink_raw_imu_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (raw) + int16_t yacc; ///< Y acceleration (raw) + int16_t zacc; ///< Z acceleration (raw) + int16_t xgyro; ///< Angular speed around X axis (raw) + int16_t ygyro; ///< Angular speed around Y axis (raw) + int16_t zgyro; ///< Angular speed around Z axis (raw) + int16_t xmag; ///< X Magnetic field (raw) + int16_t ymag; ///< Y Magnetic field (raw) + int16_t zmag; ///< Z Magnetic field (raw) +} mavlink_raw_imu_t; + +#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_ID_28_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_RAW_IMU { \ + "RAW_IMU", \ + 10, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, usec) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_imu_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_imu_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_raw_imu_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_raw_imu_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a raw_imu 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 xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_raw_imu_t packet; + packet.usec = 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; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; + return mavlink_finalize_message(msg, system_id, component_id, 26); +} + +/** + * @brief Pack a raw_imu 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 xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_raw_imu_t packet; + packet.usec = 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; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); +} + +/** + * @brief Encode a raw_imu 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 raw_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) +{ + return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); +} + +/** + * @brief Send a raw_imu message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param xacc X acceleration (raw) + * @param yacc Y acceleration (raw) + * @param zacc Z acceleration (raw) + * @param xgyro Angular speed around X axis (raw) + * @param ygyro Angular speed around Y axis (raw) + * @param zgyro Angular speed around Z axis (raw) + * @param xmag X Magnetic field (raw) + * @param ymag Y Magnetic field (raw) + * @param zmag Z Magnetic field (raw) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, 26); +#else + mavlink_raw_imu_t packet; + packet.usec = 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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, 26); +#endif +} + +#endif + +// MESSAGE RAW_IMU UNPACKING + + +/** + * @brief Get field usec from raw_imu message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_raw_imu_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field xacc from raw_imu message + * + * @return X acceleration (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field yacc from raw_imu message + * + * @return Y acceleration (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field zacc from raw_imu message + * + * @return Z acceleration (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field xgyro from raw_imu message + * + * @return Angular speed around X axis (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field ygyro from raw_imu message + * + * @return Angular speed around Y axis (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field zgyro from raw_imu message + * + * @return Angular speed around Z axis (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field xmag from raw_imu message + * + * @return X Magnetic field (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field ymag from raw_imu message + * + * @return Y Magnetic field (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field zmag from raw_imu message + * + * @return Z Magnetic field (raw) + */ +static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 24); +} + +/** + * @brief Decode a raw_imu message into a struct + * + * @param msg The message to decode + * @param raw_imu C-struct to decode the message contents into + */ +static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu) +{ +#if MAVLINK_NEED_BYTE_SWAP + raw_imu->usec = mavlink_msg_raw_imu_get_usec(msg); + raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg); + raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg); + raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg); + raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg); + raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg); + raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg); + raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); + raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); + raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); +#else + memcpy(raw_imu, _MAV_PAYLOAD(msg), 26); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_raw_pressure.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_raw_pressure.h new file mode 100644 index 000000000..b493b7e98 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_raw_pressure.h @@ -0,0 +1,232 @@ +// MESSAGE RAW_PRESSURE PACKING + +#define MAVLINK_MSG_ID_RAW_PRESSURE 29 + +typedef struct __mavlink_raw_pressure_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t press_abs; ///< Absolute pressure (raw) + int16_t press_diff1; ///< Differential pressure 1 (raw) + int16_t press_diff2; ///< Differential pressure 2 (raw) + int16_t temperature; ///< Raw Temperature measurement (raw) +} mavlink_raw_pressure_t; + +#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 +#define MAVLINK_MSG_ID_29_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_RAW_PRESSURE { \ + "RAW_PRESSURE", \ + 5, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_pressure_t, usec) }, \ + { "press_abs", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_pressure_t, press_abs) }, \ + { "press_diff1", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_pressure_t, press_diff1) }, \ + { "press_diff2", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_pressure_t, press_diff2) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_pressure_t, temperature) }, \ + } \ +} + + +/** + * @brief Pack a raw_pressure 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 press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16); +#else + mavlink_raw_pressure_t packet; + packet.usec = usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + return mavlink_finalize_message(msg, system_id, component_id, 16); +} + +/** + * @brief Pack a raw_pressure 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 press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,int16_t press_abs,int16_t press_diff1,int16_t press_diff2,int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16); +#else + mavlink_raw_pressure_t packet; + packet.usec = usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); +} + +/** + * @brief Encode a raw_pressure 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 raw_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_pressure_t* raw_pressure) +{ + return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); +} + +/** + * @brief Send a raw_pressure message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param press_abs Absolute pressure (raw) + * @param press_diff1 Differential pressure 1 (raw) + * @param press_diff2 Differential pressure 2 (raw) + * @param temperature Raw Temperature measurement (raw) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, 16); +#else + mavlink_raw_pressure_t packet; + packet.usec = usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, 16); +#endif +} + +#endif + +// MESSAGE RAW_PRESSURE UNPACKING + + +/** + * @brief Get field usec from raw_pressure message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_raw_pressure_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field press_abs from raw_pressure message + * + * @return Absolute pressure (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field press_diff1 from raw_pressure message + * + * @return Differential pressure 1 (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field press_diff2 from raw_pressure message + * + * @return Differential pressure 2 (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field temperature from raw_pressure message + * + * @return Raw Temperature measurement (raw) + */ +static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Decode a raw_pressure message into a struct + * + * @param msg The message to decode + * @param raw_pressure C-struct to decode the message contents into + */ +static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, mavlink_raw_pressure_t* raw_pressure) +{ +#if MAVLINK_NEED_BYTE_SWAP + raw_pressure->usec = mavlink_msg_raw_pressure_get_usec(msg); + raw_pressure->press_abs = mavlink_msg_raw_pressure_get_press_abs(msg); + raw_pressure->press_diff1 = mavlink_msg_raw_pressure_get_press_diff1(msg); + raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg); + raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg); +#else + memcpy(raw_pressure, _MAV_PAYLOAD(msg), 16); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_override.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_override.h new file mode 100644 index 000000000..30e5a8f98 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_override.h @@ -0,0 +1,342 @@ +// MESSAGE RC_CHANNELS_OVERRIDE PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 + +typedef struct __mavlink_rc_channels_override_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds +} mavlink_rc_channels_override_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_ID_70_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ + "RC_CHANNELS_OVERRIDE", \ + 10, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rc_channels_override_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rc_channels_override_t, target_component) }, \ + { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_override_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_override_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_override_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels_override 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 chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, chan1_raw); + _mav_put_uint16_t(buf, 4, chan2_raw); + _mav_put_uint16_t(buf, 6, chan3_raw); + _mav_put_uint16_t(buf, 8, chan4_raw); + _mav_put_uint16_t(buf, 10, chan5_raw); + _mav_put_uint16_t(buf, 12, chan6_raw); + _mav_put_uint16_t(buf, 14, chan7_raw); + _mav_put_uint16_t(buf, 16, chan8_raw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_rc_channels_override_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @brief Pack a rc_channels_override 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 chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_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 chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, chan1_raw); + _mav_put_uint16_t(buf, 4, chan2_raw); + _mav_put_uint16_t(buf, 6, chan3_raw); + _mav_put_uint16_t(buf, 8, chan4_raw); + _mav_put_uint16_t(buf, 10, chan5_raw); + _mav_put_uint16_t(buf, 12, chan6_raw); + _mav_put_uint16_t(buf, 14, chan7_raw); + _mav_put_uint16_t(buf, 16, chan8_raw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_rc_channels_override_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); +} + +/** + * @brief Encode a rc_channels_override 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 rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** + * @brief Send a rc_channels_override message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, chan1_raw); + _mav_put_uint16_t(buf, 4, chan2_raw); + _mav_put_uint16_t(buf, 6, chan3_raw); + _mav_put_uint16_t(buf, 8, chan4_raw); + _mav_put_uint16_t(buf, 10, chan5_raw); + _mav_put_uint16_t(buf, 12, chan6_raw); + _mav_put_uint16_t(buf, 14, chan7_raw); + _mav_put_uint16_t(buf, 16, chan8_raw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, 18); +#else + mavlink_rc_channels_override_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, 18); +#endif +} + +#endif + +// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING + + +/** + * @brief Get field target_system from rc_channels_override message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from rc_channels_override message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field chan1_raw from rc_channels_override message + * + * @return RC channel 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field chan2_raw from rc_channels_override message + * + * @return RC channel 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan3_raw from rc_channels_override message + * + * @return RC channel 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan4_raw from rc_channels_override message + * + * @return RC channel 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan5_raw from rc_channels_override message + * + * @return RC channel 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan6_raw from rc_channels_override message + * + * @return RC channel 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan7_raw from rc_channels_override message + * + * @return RC channel 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field chan8_raw from rc_channels_override message + * + * @return RC channel 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Decode a rc_channels_override message into a struct + * + * @param msg The message to decode + * @param rc_channels_override C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); + rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); + rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg); + rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg); + rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg); + rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg); + rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg); + rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg); + rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg); + rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); +#else + memcpy(rc_channels_override, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_raw.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_raw.h new file mode 100644 index 000000000..855f7cced --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_raw.h @@ -0,0 +1,320 @@ +// MESSAGE RC_CHANNELS_RAW PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 + +typedef struct __mavlink_rc_channels_raw_t +{ + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% +} mavlink_rc_channels_raw_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 17 +#define MAVLINK_MSG_ID_35_LEN 17 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW { \ + "RC_CHANNELS_RAW", \ + 9, \ + { { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_raw_t, chan1_raw) }, \ + { "chan2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_raw_t, chan2_raw) }, \ + { "chan3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_raw_t, chan3_raw) }, \ + { "chan4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_raw_t, chan4_raw) }, \ + { "chan5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_raw_t, chan5_raw) }, \ + { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_raw_t, chan6_raw) }, \ + { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_raw_t, chan7_raw) }, \ + { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_raw_t, chan8_raw) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_raw_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels_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 chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[17]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 17); +#else + mavlink_rc_channels_raw_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + return mavlink_finalize_message(msg, system_id, component_id, 17); +} + +/** + * @brief Pack a rc_channels_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 chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[17]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 17); +#else + mavlink_rc_channels_raw_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17); +} + +/** + * @brief Encode a rc_channels_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 rc_channels_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_raw_t* rc_channels_raw) +{ + return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); +} + +/** + * @brief Send a rc_channels_raw message + * @param chan MAVLink channel to send the message + * + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[17]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, 17); +#else + mavlink_rc_channels_raw_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, 17); +#endif +} + +#endif + +// MESSAGE RC_CHANNELS_RAW UNPACKING + + +/** + * @brief Get field chan1_raw from rc_channels_raw message + * + * @return RC channel 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field chan2_raw from rc_channels_raw message + * + * @return RC channel 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field chan3_raw from rc_channels_raw message + * + * @return RC channel 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field chan4_raw from rc_channels_raw message + * + * @return RC channel 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field chan5_raw from rc_channels_raw message + * + * @return RC channel 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field chan6_raw from rc_channels_raw message + * + * @return RC channel 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field chan7_raw from rc_channels_raw message + * + * @return RC channel 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field chan8_raw from rc_channels_raw message + * + * @return RC channel 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field rssi from rc_channels_raw message + * + * @return Receive signal strength indicator, 0: 0%, 255: 100% + */ +static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Decode a rc_channels_raw message into a struct + * + * @param msg The message to decode + * @param rc_channels_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* msg, mavlink_rc_channels_raw_t* rc_channels_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); + rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); + rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); + rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); + rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); + rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); + rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); + rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); + rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); +#else + memcpy(rc_channels_raw, _MAV_PAYLOAD(msg), 17); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_scaled.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_scaled.h new file mode 100644 index 000000000..49df14a7a --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_rc_channels_scaled.h @@ -0,0 +1,320 @@ +// MESSAGE RC_CHANNELS_SCALED PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 + +typedef struct __mavlink_rc_channels_scaled_t +{ + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% +} mavlink_rc_channels_scaled_t; + +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 17 +#define MAVLINK_MSG_ID_36_LEN 17 + + + +#define MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED { \ + "RC_CHANNELS_SCALED", \ + 9, \ + { { "chan1_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_rc_channels_scaled_t, chan1_scaled) }, \ + { "chan2_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_rc_channels_scaled_t, chan2_scaled) }, \ + { "chan3_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_rc_channels_scaled_t, chan3_scaled) }, \ + { "chan4_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_rc_channels_scaled_t, chan4_scaled) }, \ + { "chan5_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rc_channels_scaled_t, chan5_scaled) }, \ + { "chan6_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rc_channels_scaled_t, chan6_scaled) }, \ + { "chan7_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_rc_channels_scaled_t, chan7_scaled) }, \ + { "chan8_scaled", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_rc_channels_scaled_t, chan8_scaled) }, \ + { "rssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_scaled_t, rssi) }, \ + } \ +} + + +/** + * @brief Pack a rc_channels_scaled 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 chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[17]; + _mav_put_int16_t(buf, 0, chan1_scaled); + _mav_put_int16_t(buf, 2, chan2_scaled); + _mav_put_int16_t(buf, 4, chan3_scaled); + _mav_put_int16_t(buf, 6, chan4_scaled); + _mav_put_int16_t(buf, 8, chan5_scaled); + _mav_put_int16_t(buf, 10, chan6_scaled); + _mav_put_int16_t(buf, 12, chan7_scaled); + _mav_put_int16_t(buf, 14, chan8_scaled); + _mav_put_uint8_t(buf, 16, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 17); +#else + mavlink_rc_channels_scaled_t packet; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + return mavlink_finalize_message(msg, system_id, component_id, 17); +} + +/** + * @brief Pack a rc_channels_scaled 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 chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int16_t chan1_scaled,int16_t chan2_scaled,int16_t chan3_scaled,int16_t chan4_scaled,int16_t chan5_scaled,int16_t chan6_scaled,int16_t chan7_scaled,int16_t chan8_scaled,uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[17]; + _mav_put_int16_t(buf, 0, chan1_scaled); + _mav_put_int16_t(buf, 2, chan2_scaled); + _mav_put_int16_t(buf, 4, chan3_scaled); + _mav_put_int16_t(buf, 6, chan4_scaled); + _mav_put_int16_t(buf, 8, chan5_scaled); + _mav_put_int16_t(buf, 10, chan6_scaled); + _mav_put_int16_t(buf, 12, chan7_scaled); + _mav_put_int16_t(buf, 14, chan8_scaled); + _mav_put_uint8_t(buf, 16, rssi); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 17); +#else + mavlink_rc_channels_scaled_t packet; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 17); +#endif + + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17); +} + +/** + * @brief Encode a rc_channels_scaled 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 rc_channels_scaled C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ + return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); +} + +/** + * @brief Send a rc_channels_scaled message + * @param chan MAVLink channel to send the message + * + * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[17]; + _mav_put_int16_t(buf, 0, chan1_scaled); + _mav_put_int16_t(buf, 2, chan2_scaled); + _mav_put_int16_t(buf, 4, chan3_scaled); + _mav_put_int16_t(buf, 6, chan4_scaled); + _mav_put_int16_t(buf, 8, chan5_scaled); + _mav_put_int16_t(buf, 10, chan6_scaled); + _mav_put_int16_t(buf, 12, chan7_scaled); + _mav_put_int16_t(buf, 14, chan8_scaled); + _mav_put_uint8_t(buf, 16, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, 17); +#else + mavlink_rc_channels_scaled_t packet; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, 17); +#endif +} + +#endif + +// MESSAGE RC_CHANNELS_SCALED UNPACKING + + +/** + * @brief Get field chan1_scaled from rc_channels_scaled message + * + * @return RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Get field chan2_scaled from rc_channels_scaled message + * + * @return RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 2); +} + +/** + * @brief Get field chan3_scaled from rc_channels_scaled message + * + * @return RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 4); +} + +/** + * @brief Get field chan4_scaled from rc_channels_scaled message + * + * @return RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 6); +} + +/** + * @brief Get field chan5_scaled from rc_channels_scaled message + * + * @return RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field chan6_scaled from rc_channels_scaled message + * + * @return RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field chan7_scaled from rc_channels_scaled message + * + * @return RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field chan8_scaled from rc_channels_scaled message + * + * @return RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + */ +static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field rssi from rc_channels_scaled message + * + * @return Receive signal strength indicator, 0: 0%, 255: 100% + */ +static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Decode a rc_channels_scaled message into a struct + * + * @param msg The message to decode + * @param rc_channels_scaled C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t* msg, mavlink_rc_channels_scaled_t* rc_channels_scaled) +{ +#if MAVLINK_NEED_BYTE_SWAP + rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg); + rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg); + rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg); + rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg); + rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg); + rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg); + rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg); + rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg); + rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); +#else + memcpy(rc_channels_scaled, _MAV_PAYLOAD(msg), 17); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_request_data_stream.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_request_data_stream.h new file mode 100644 index 000000000..e80473aa1 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_request_data_stream.h @@ -0,0 +1,232 @@ +// MESSAGE REQUEST_DATA_STREAM PACKING + +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 + +typedef struct __mavlink_request_data_stream_t +{ + uint8_t target_system; ///< The target requested to send the message stream. + uint8_t target_component; ///< The target requested to send the message stream. + uint8_t req_stream_id; ///< The ID of the requested message type + uint16_t req_message_rate; ///< Update rate in Hertz + uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. +} mavlink_request_data_stream_t; + +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 +#define MAVLINK_MSG_ID_66_LEN 6 + + + +#define MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM { \ + "REQUEST_DATA_STREAM", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_request_data_stream_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_request_data_stream_t, target_component) }, \ + { "req_stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_request_data_stream_t, req_stream_id) }, \ + { "req_message_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_request_data_stream_t, req_message_rate) }, \ + { "start_stop", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_data_stream_t, start_stop) }, \ + } \ +} + + +/** + * @brief Pack a request_data_stream 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 The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested message type + * @param req_message_rate Update rate in Hertz + * @param start_stop 1 to start sending, 0 to stop sending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, req_stream_id); + _mav_put_uint16_t(buf, 3, req_message_rate); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 6); +#else + mavlink_request_data_stream_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.req_message_rate = req_message_rate; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + return mavlink_finalize_message(msg, system_id, component_id, 6); +} + +/** + * @brief Pack a request_data_stream 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 The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested message type + * @param req_message_rate Update rate in Hertz + * @param start_stop 1 to start sending, 0 to stop sending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_request_data_stream_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 req_stream_id,uint16_t req_message_rate,uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, req_stream_id); + _mav_put_uint16_t(buf, 3, req_message_rate); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 6); +#else + mavlink_request_data_stream_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.req_message_rate = req_message_rate; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 6); +#endif + + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6); +} + +/** + * @brief Encode a request_data_stream 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 request_data_stream C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_data_stream_t* request_data_stream) +{ + return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); +} + +/** + * @brief Send a request_data_stream message + * @param chan MAVLink channel to send the message + * + * @param target_system The target requested to send the message stream. + * @param target_component The target requested to send the message stream. + * @param req_stream_id The ID of the requested message type + * @param req_message_rate Update rate in Hertz + * @param start_stop 1 to start sending, 0 to stop sending. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, req_stream_id); + _mav_put_uint16_t(buf, 3, req_message_rate); + _mav_put_uint8_t(buf, 5, start_stop); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, 6); +#else + mavlink_request_data_stream_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.req_message_rate = req_message_rate; + packet.start_stop = start_stop; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, 6); +#endif +} + +#endif + +// MESSAGE REQUEST_DATA_STREAM UNPACKING + + +/** + * @brief Get field target_system from request_data_stream message + * + * @return The target requested to send the message stream. + */ +static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from request_data_stream message + * + * @return The target requested to send the message stream. + */ +static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field req_stream_id from request_data_stream message + * + * @return The ID of the requested message type + */ +static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field req_message_rate from request_data_stream message + * + * @return Update rate in Hertz + */ +static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 3); +} + +/** + * @brief Get field start_stop from request_data_stream message + * + * @return 1 to start sending, 0 to stop sending. + */ +static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Decode a request_data_stream message into a struct + * + * @param msg The message to decode + * @param request_data_stream C-struct to decode the message contents into + */ +static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_t* msg, mavlink_request_data_stream_t* request_data_stream) +{ +#if MAVLINK_NEED_BYTE_SWAP + request_data_stream->target_system = mavlink_msg_request_data_stream_get_target_system(msg); + request_data_stream->target_component = mavlink_msg_request_data_stream_get_target_component(msg); + request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); + request_data_stream->req_message_rate = mavlink_msg_request_data_stream_get_req_message_rate(msg); + request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); +#else + memcpy(request_data_stream, _MAV_PAYLOAD(msg), 6); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h new file mode 100644 index 000000000..b344f3aa2 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -0,0 +1,232 @@ +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 58 + +typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t +{ + uint64_t time_us; ///< Timestamp in micro seconds since unix epoch + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 +} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t; + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 24 +#define MAVLINK_MSG_ID_58_LEN 24 + + + +#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT { \ + "ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT", \ + 5, \ + { { "time_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, time_us) }, \ + { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, roll_speed) }, \ + { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, pitch_speed) }, \ + { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, yaw_speed) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, thrust) }, \ + } \ +} + + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint 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_us Timestamp in micro seconds since unix epoch + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll_speed); + _mav_put_float(buf, 12, pitch_speed); + _mav_put_float(buf, 16, yaw_speed); + _mav_put_float(buf, 20, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; + packet.time_us = time_us; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + return mavlink_finalize_message(msg, system_id, component_id, 24); +} + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint 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_us Timestamp in micro seconds since unix epoch + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_us,float roll_speed,float pitch_speed,float yaw_speed,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll_speed); + _mav_put_float(buf, 12, pitch_speed); + _mav_put_float(buf, 16, yaw_speed); + _mav_put_float(buf, 20, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; + packet.time_us = time_us; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24); +} + +/** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint 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 roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_us, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); +} + +/** + * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll_speed); + _mav_put_float(buf, 12, pitch_speed); + _mav_put_float(buf, 16, yaw_speed); + _mav_put_float(buf, 20, thrust); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, 24); +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; + packet.time_us = time_us; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, 24); +#endif +} + +#endif + +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING + + +/** + * @brief Get field time_us from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Timestamp in micro seconds since unix epoch + */ +static inline uint64_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ +#if MAVLINK_NEED_BYTE_SWAP + roll_pitch_yaw_speed_thrust_setpoint->time_us = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(msg); + roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg); + roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg); +#else + memcpy(roll_pitch_yaw_speed_thrust_setpoint, _MAV_PAYLOAD(msg), 24); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h new file mode 100644 index 000000000..16155f134 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -0,0 +1,232 @@ +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 57 + +typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t +{ + uint64_t time_us; ///< Timestamp in micro seconds since unix epoch + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 +} mavlink_roll_pitch_yaw_thrust_setpoint_t; + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 24 +#define MAVLINK_MSG_ID_57_LEN 24 + + + +#define MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT { \ + "ROLL_PITCH_YAW_THRUST_SETPOINT", \ + 5, \ + { { "time_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, time_us) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, yaw) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, thrust) }, \ + } \ +} + + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint 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_us Timestamp in micro seconds since unix epoch + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_us, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t packet; + packet.time_us = time_us; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return mavlink_finalize_message(msg, system_id, component_id, 24); +} + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint 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_us Timestamp in micro seconds since unix epoch + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_us,float roll,float pitch,float yaw,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t packet; + packet.time_us = time_us; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); +#endif + + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24); +} + +/** + * @brief Encode a roll_pitch_yaw_thrust_setpoint 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 roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_us, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); +} + +/** + * @brief Send a roll_pitch_yaw_thrust_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_us Timestamp in micro seconds since unix epoch + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint64_t time_us, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint64_t(buf, 0, time_us); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, thrust); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, 24); +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t packet; + packet.time_us = time_us; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, 24); +#endif +} + +#endif + +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING + + +/** + * @brief Get field time_us from roll_pitch_yaw_thrust_setpoint message + * + * @return Timestamp in micro seconds since unix epoch + */ +static inline uint64_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ +#if MAVLINK_NEED_BYTE_SWAP + roll_pitch_yaw_thrust_setpoint->time_us = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(msg); + roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg); + roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg); + roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg); + roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg); +#else + memcpy(roll_pitch_yaw_thrust_setpoint, _MAV_PAYLOAD(msg), 24); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_safety_allowed_area.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_safety_allowed_area.h new file mode 100644 index 000000000..07b91fd0e --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_safety_allowed_area.h @@ -0,0 +1,276 @@ +// MESSAGE SAFETY_ALLOWED_AREA PACKING + +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 + +typedef struct __mavlink_safety_allowed_area_t +{ + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 +} mavlink_safety_allowed_area_t; + +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 +#define MAVLINK_MSG_ID_54_LEN 25 + + + +#define MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA { \ + "SAFETY_ALLOWED_AREA", \ + 7, \ + { { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_safety_allowed_area_t, frame) }, \ + { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 1, offsetof(mavlink_safety_allowed_area_t, p1x) }, \ + { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_safety_allowed_area_t, p1y) }, \ + { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_safety_allowed_area_t, p1z) }, \ + { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 13, offsetof(mavlink_safety_allowed_area_t, p2x) }, \ + { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 17, offsetof(mavlink_safety_allowed_area_t, p2y) }, \ + { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 21, offsetof(mavlink_safety_allowed_area_t, p2z) }, \ + } \ +} + + +/** + * @brief Pack a safety_allowed_area message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_uint8_t(buf, 0, frame); + _mav_put_float(buf, 1, p1x); + _mav_put_float(buf, 5, p1y); + _mav_put_float(buf, 9, p1z); + _mav_put_float(buf, 13, p2x); + _mav_put_float(buf, 17, p2y); + _mav_put_float(buf, 21, p2z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); +#else + mavlink_safety_allowed_area_t packet; + packet.frame = frame; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + return mavlink_finalize_message(msg, system_id, component_id, 25); +} + +/** + * @brief Pack a safety_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_uint8_t(buf, 0, frame); + _mav_put_float(buf, 1, p1x); + _mav_put_float(buf, 5, p1y); + _mav_put_float(buf, 9, p1z); + _mav_put_float(buf, 13, p2x); + _mav_put_float(buf, 17, p2y); + _mav_put_float(buf, 21, p2z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 25); +#else + mavlink_safety_allowed_area_t packet; + packet.frame = frame; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 25); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25); +} + +/** + * @brief Encode a safety_allowed_area struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param safety_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_allowed_area_t* safety_allowed_area) +{ + return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); +} + +/** + * @brief Send a safety_allowed_area message + * @param chan MAVLink channel to send the message + * + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_uint8_t(buf, 0, frame); + _mav_put_float(buf, 1, p1x); + _mav_put_float(buf, 5, p1y); + _mav_put_float(buf, 9, p1z); + _mav_put_float(buf, 13, p2x); + _mav_put_float(buf, 17, p2y); + _mav_put_float(buf, 21, p2z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, 25); +#else + mavlink_safety_allowed_area_t packet; + packet.frame = frame; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, 25); +#endif +} + +#endif + +// MESSAGE SAFETY_ALLOWED_AREA UNPACKING + + +/** + * @brief Get field frame from safety_allowed_area message + * + * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + */ +static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field p1x from safety_allowed_area message + * + * @return x position 1 / Latitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 1); +} + +/** + * @brief Get field p1y from safety_allowed_area message + * + * @return y position 1 / Longitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 5); +} + +/** + * @brief Get field p1z from safety_allowed_area message + * + * @return z position 1 / Altitude 1 + */ +static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 9); +} + +/** + * @brief Get field p2x from safety_allowed_area message + * + * @return x position 2 / Latitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 13); +} + +/** + * @brief Get field p2y from safety_allowed_area message + * + * @return y position 2 / Longitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 17); +} + +/** + * @brief Get field p2z from safety_allowed_area message + * + * @return z position 2 / Altitude 2 + */ +static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 21); +} + +/** + * @brief Decode a safety_allowed_area message into a struct + * + * @param msg The message to decode + * @param safety_allowed_area C-struct to decode the message contents into + */ +static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_allowed_area_t* safety_allowed_area) +{ +#if MAVLINK_NEED_BYTE_SWAP + safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); + safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); + safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); + safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); + safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); + safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); + safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); +#else + memcpy(safety_allowed_area, _MAV_PAYLOAD(msg), 25); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_safety_set_allowed_area.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_safety_set_allowed_area.h new file mode 100644 index 000000000..3c122227a --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_safety_set_allowed_area.h @@ -0,0 +1,320 @@ +// MESSAGE SAFETY_SET_ALLOWED_AREA PACKING + +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 + +typedef struct __mavlink_safety_set_allowed_area_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 +} mavlink_safety_set_allowed_area_t; + +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 +#define MAVLINK_MSG_ID_53_LEN 27 + + + +#define MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA { \ + "SAFETY_SET_ALLOWED_AREA", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_safety_set_allowed_area_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_safety_set_allowed_area_t, target_component) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_safety_set_allowed_area_t, frame) }, \ + { "p1x", NULL, MAVLINK_TYPE_FLOAT, 0, 3, offsetof(mavlink_safety_set_allowed_area_t, p1x) }, \ + { "p1y", NULL, MAVLINK_TYPE_FLOAT, 0, 7, offsetof(mavlink_safety_set_allowed_area_t, p1y) }, \ + { "p1z", NULL, MAVLINK_TYPE_FLOAT, 0, 11, offsetof(mavlink_safety_set_allowed_area_t, p1z) }, \ + { "p2x", NULL, MAVLINK_TYPE_FLOAT, 0, 15, offsetof(mavlink_safety_set_allowed_area_t, p2x) }, \ + { "p2y", NULL, MAVLINK_TYPE_FLOAT, 0, 19, offsetof(mavlink_safety_set_allowed_area_t, p2y) }, \ + { "p2z", NULL, MAVLINK_TYPE_FLOAT, 0, 23, offsetof(mavlink_safety_set_allowed_area_t, p2z) }, \ + } \ +} + + +/** + * @brief Pack a safety_set_allowed_area message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[27]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, frame); + _mav_put_float(buf, 3, p1x); + _mav_put_float(buf, 7, p1y); + _mav_put_float(buf, 11, p1z); + _mav_put_float(buf, 15, p2x); + _mav_put_float(buf, 19, p2y); + _mav_put_float(buf, 23, p2z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 27); +#else + mavlink_safety_set_allowed_area_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 27); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + return mavlink_finalize_message(msg, system_id, component_id, 27); +} + +/** + * @brief Pack a safety_set_allowed_area 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 frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[27]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, frame); + _mav_put_float(buf, 3, p1x); + _mav_put_float(buf, 7, p1y); + _mav_put_float(buf, 11, p1z); + _mav_put_float(buf, 15, p2x); + _mav_put_float(buf, 19, p2y); + _mav_put_float(buf, 23, p2z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 27); +#else + mavlink_safety_set_allowed_area_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 27); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 27); +} + +/** + * @brief Encode a safety_set_allowed_area struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param safety_set_allowed_area C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ + return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); +} + +/** + * @brief Send a safety_set_allowed_area message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + * @param p1x x position 1 / Latitude 1 + * @param p1y y position 1 / Longitude 1 + * @param p1z z position 1 / Altitude 1 + * @param p2x x position 2 / Latitude 2 + * @param p2y y position 2 / Longitude 2 + * @param p2z z position 2 / Altitude 2 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[27]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, frame); + _mav_put_float(buf, 3, p1x); + _mav_put_float(buf, 7, p1y); + _mav_put_float(buf, 11, p1z); + _mav_put_float(buf, 15, p2x); + _mav_put_float(buf, 19, p2y); + _mav_put_float(buf, 23, p2z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, 27); +#else + mavlink_safety_set_allowed_area_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, 27); +#endif +} + +#endif + +// MESSAGE SAFETY_SET_ALLOWED_AREA UNPACKING + + +/** + * @brief Get field target_system from safety_set_allowed_area message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from safety_set_allowed_area message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field frame from safety_set_allowed_area message + * + * @return Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + */ +static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field p1x from safety_set_allowed_area message + * + * @return x position 1 / Latitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 3); +} + +/** + * @brief Get field p1y from safety_set_allowed_area message + * + * @return y position 1 / Longitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 7); +} + +/** + * @brief Get field p1z from safety_set_allowed_area message + * + * @return z position 1 / Altitude 1 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 11); +} + +/** + * @brief Get field p2x from safety_set_allowed_area message + * + * @return x position 2 / Latitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 15); +} + +/** + * @brief Get field p2y from safety_set_allowed_area message + * + * @return y position 2 / Longitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 19); +} + +/** + * @brief Get field p2z from safety_set_allowed_area message + * + * @return z position 2 / Altitude 2 + */ +static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 23); +} + +/** + * @brief Decode a safety_set_allowed_area message into a struct + * + * @param msg The message to decode + * @param safety_set_allowed_area C-struct to decode the message contents into + */ +static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_message_t* msg, mavlink_safety_set_allowed_area_t* safety_set_allowed_area) +{ +#if MAVLINK_NEED_BYTE_SWAP + safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); + safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); + safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); + safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); + safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); + safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); + safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); + safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); + safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); +#else + memcpy(safety_set_allowed_area, _MAV_PAYLOAD(msg), 27); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_scaled_imu.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_scaled_imu.h new file mode 100644 index 000000000..4315dac27 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_scaled_imu.h @@ -0,0 +1,342 @@ +// MESSAGE SCALED_IMU PACKING + +#define MAVLINK_MSG_ID_SCALED_IMU 26 + +typedef struct __mavlink_scaled_imu_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) +} mavlink_scaled_imu_t; + +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 26 +#define MAVLINK_MSG_ID_26_LEN 26 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ + "SCALED_IMU", \ + 10, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_scaled_imu_t, usec) }, \ + { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu_t, xacc) }, \ + { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu_t, yacc) }, \ + { "zacc", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu_t, zacc) }, \ + { "xgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu_t, xgyro) }, \ + { "ygyro", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, ygyro) }, \ + { "zgyro", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, zgyro) }, \ + { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, xmag) }, \ + { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, ymag) }, \ + { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_scaled_imu_t, zmag) }, \ + } \ +} + + +/** + * @brief Pack a scaled_imu 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 xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_scaled_imu_t packet; + packet.usec = 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; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; + return mavlink_finalize_message(msg, system_id, component_id, 26); +} + +/** + * @brief Pack a scaled_imu 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 xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 26); +#else + mavlink_scaled_imu_t packet; + packet.usec = 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; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); +} + +/** + * @brief Encode a scaled_imu 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 scaled_imu C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) +{ + return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->usec, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); +} + +/** + * @brief Send a scaled_imu message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @param xgyro Angular speed around X axis (millirad /sec) + * @param ygyro Angular speed around Y axis (millirad /sec) + * @param zgyro Angular speed around Z axis (millirad /sec) + * @param xmag X Magnetic field (milli tesla) + * @param ymag Y Magnetic field (milli tesla) + * @param zmag Z Magnetic field (milli tesla) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, 26); +#else + mavlink_scaled_imu_t packet; + packet.usec = 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; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, 26); +#endif +} + +#endif + +// MESSAGE SCALED_IMU UNPACKING + + +/** + * @brief Get field usec from scaled_imu message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_scaled_imu_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field xacc from scaled_imu message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field yacc from scaled_imu message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 10); +} + +/** + * @brief Get field zacc from scaled_imu message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 12); +} + +/** + * @brief Get field xgyro from scaled_imu message + * + * @return Angular speed around X axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + +/** + * @brief Get field ygyro from scaled_imu message + * + * @return Angular speed around Y axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Get field zgyro from scaled_imu message + * + * @return Angular speed around Z axis (millirad /sec) + */ +static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 18); +} + +/** + * @brief Get field xmag from scaled_imu message + * + * @return X Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 20); +} + +/** + * @brief Get field ymag from scaled_imu message + * + * @return Y Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + +/** + * @brief Get field zmag from scaled_imu message + * + * @return Z Magnetic field (milli tesla) + */ +static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 24); +} + +/** + * @brief Decode a scaled_imu message into a struct + * + * @param msg The message to decode + * @param scaled_imu C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, mavlink_scaled_imu_t* scaled_imu) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_imu->usec = mavlink_msg_scaled_imu_get_usec(msg); + scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg); + scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg); + scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg); + scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg); + scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg); + scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg); + scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); + scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); + scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); +#else + memcpy(scaled_imu, _MAV_PAYLOAD(msg), 26); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_scaled_pressure.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_scaled_pressure.h new file mode 100644 index 000000000..579f7f4ee --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_scaled_pressure.h @@ -0,0 +1,210 @@ +// MESSAGE SCALED_PRESSURE PACKING + +#define MAVLINK_MSG_ID_SCALED_PRESSURE 38 + +typedef struct __mavlink_scaled_pressure_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float press_abs; ///< Absolute pressure (hectopascal) + float press_diff; ///< Differential pressure 1 (hectopascal) + int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) +} mavlink_scaled_pressure_t; + +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 18 +#define MAVLINK_MSG_ID_38_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ + "SCALED_PRESSURE", \ + 4, \ + { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_scaled_pressure_t, usec) }, \ + { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ + { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + } \ +} + + +/** + * @brief Pack a scaled_pressure 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 press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, press_abs); + _mav_put_float(buf, 12, press_diff); + _mav_put_int16_t(buf, 16, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_scaled_pressure_t packet; + packet.usec = usec; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @brief Pack a scaled_pressure 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 press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float press_abs,float press_diff,int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, press_abs); + _mav_put_float(buf, 12, press_diff); + _mav_put_int16_t(buf, 16, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_scaled_pressure_t packet; + packet.usec = usec; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); +} + +/** + * @brief Encode a scaled_pressure 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 scaled_pressure C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) +{ + return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->usec, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); +} + +/** + * @brief Send a scaled_pressure message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param press_abs Absolute pressure (hectopascal) + * @param press_diff Differential pressure 1 (hectopascal) + * @param temperature Temperature measurement (0.01 degrees celsius) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, press_abs); + _mav_put_float(buf, 12, press_diff); + _mav_put_int16_t(buf, 16, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, 18); +#else + mavlink_scaled_pressure_t packet; + packet.usec = usec; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, 18); +#endif +} + +#endif + +// MESSAGE SCALED_PRESSURE UNPACKING + + +/** + * @brief Get field usec from scaled_pressure message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_scaled_pressure_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field press_abs from scaled_pressure message + * + * @return Absolute pressure (hectopascal) + */ +static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field press_diff from scaled_pressure message + * + * @return Differential pressure 1 (hectopascal) + */ +static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field temperature from scaled_pressure message + * + * @return Temperature measurement (0.01 degrees celsius) + */ +static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 16); +} + +/** + * @brief Decode a scaled_pressure message into a struct + * + * @param msg The message to decode + * @param scaled_pressure C-struct to decode the message contents into + */ +static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* msg, mavlink_scaled_pressure_t* scaled_pressure) +{ +#if MAVLINK_NEED_BYTE_SWAP + scaled_pressure->usec = mavlink_msg_scaled_pressure_get_usec(msg); + scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); + scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); + scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); +#else + memcpy(scaled_pressure, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_servo_output_raw.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_servo_output_raw.h new file mode 100644 index 000000000..b5be3318c --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_servo_output_raw.h @@ -0,0 +1,298 @@ +// MESSAGE SERVO_OUTPUT_RAW PACKING + +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 + +typedef struct __mavlink_servo_output_raw_t +{ + uint16_t servo1_raw; ///< Servo output 1 value, in microseconds + uint16_t servo2_raw; ///< Servo output 2 value, in microseconds + uint16_t servo3_raw; ///< Servo output 3 value, in microseconds + uint16_t servo4_raw; ///< Servo output 4 value, in microseconds + uint16_t servo5_raw; ///< Servo output 5 value, in microseconds + uint16_t servo6_raw; ///< Servo output 6 value, in microseconds + uint16_t servo7_raw; ///< Servo output 7 value, in microseconds + uint16_t servo8_raw; ///< Servo output 8 value, in microseconds +} mavlink_servo_output_raw_t; + +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 16 +#define MAVLINK_MSG_ID_37_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ + "SERVO_OUTPUT_RAW", \ + 8, \ + { { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ + { "servo2_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \ + { "servo3_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \ + { "servo4_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \ + { "servo5_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \ + { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ + { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ + { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + } \ +} + + +/** + * @brief Pack a servo_output_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint16_t(buf, 0, servo1_raw); + _mav_put_uint16_t(buf, 2, servo2_raw); + _mav_put_uint16_t(buf, 4, servo3_raw); + _mav_put_uint16_t(buf, 6, servo4_raw); + _mav_put_uint16_t(buf, 8, servo5_raw); + _mav_put_uint16_t(buf, 10, servo6_raw); + _mav_put_uint16_t(buf, 12, servo7_raw); + _mav_put_uint16_t(buf, 14, servo8_raw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16); +#else + mavlink_servo_output_raw_t packet; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + return mavlink_finalize_message(msg, system_id, component_id, 16); +} + +/** + * @brief Pack a servo_output_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 servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint16_t(buf, 0, servo1_raw); + _mav_put_uint16_t(buf, 2, servo2_raw); + _mav_put_uint16_t(buf, 4, servo3_raw); + _mav_put_uint16_t(buf, 6, servo4_raw); + _mav_put_uint16_t(buf, 8, servo5_raw); + _mav_put_uint16_t(buf, 10, servo6_raw); + _mav_put_uint16_t(buf, 12, servo7_raw); + _mav_put_uint16_t(buf, 14, servo8_raw); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 16); +#else + mavlink_servo_output_raw_t packet; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); +} + +/** + * @brief Encode a servo_output_raw struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param servo_output_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) +{ + return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); +} + +/** + * @brief Send a servo_output_raw message + * @param chan MAVLink channel to send the message + * + * @param servo1_raw Servo output 1 value, in microseconds + * @param servo2_raw Servo output 2 value, in microseconds + * @param servo3_raw Servo output 3 value, in microseconds + * @param servo4_raw Servo output 4 value, in microseconds + * @param servo5_raw Servo output 5 value, in microseconds + * @param servo6_raw Servo output 6 value, in microseconds + * @param servo7_raw Servo output 7 value, in microseconds + * @param servo8_raw Servo output 8 value, in microseconds + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint16_t(buf, 0, servo1_raw); + _mav_put_uint16_t(buf, 2, servo2_raw); + _mav_put_uint16_t(buf, 4, servo3_raw); + _mav_put_uint16_t(buf, 6, servo4_raw); + _mav_put_uint16_t(buf, 8, servo5_raw); + _mav_put_uint16_t(buf, 10, servo6_raw); + _mav_put_uint16_t(buf, 12, servo7_raw); + _mav_put_uint16_t(buf, 14, servo8_raw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, 16); +#else + mavlink_servo_output_raw_t packet; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, 16); +#endif +} + +#endif + +// MESSAGE SERVO_OUTPUT_RAW UNPACKING + + +/** + * @brief Get field servo1_raw from servo_output_raw message + * + * @return Servo output 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field servo2_raw from servo_output_raw message + * + * @return Servo output 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field servo3_raw from servo_output_raw message + * + * @return Servo output 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field servo4_raw from servo_output_raw message + * + * @return Servo output 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 6); +} + +/** + * @brief Get field servo5_raw from servo_output_raw message + * + * @return Servo output 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field servo6_raw from servo_output_raw message + * + * @return Servo output 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field servo7_raw from servo_output_raw message + * + * @return Servo output 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field servo8_raw from servo_output_raw message + * + * @return Servo output 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Decode a servo_output_raw message into a struct + * + * @param msg The message to decode + * @param servo_output_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* msg, mavlink_servo_output_raw_t* servo_output_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); + servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); + servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); + servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); + servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); + servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); + servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); + servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); +#else + memcpy(servo_output_raw, _MAV_PAYLOAD(msg), 16); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_altitude.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_altitude.h new file mode 100644 index 000000000..72cc5e7b0 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_altitude.h @@ -0,0 +1,166 @@ +// MESSAGE SET_ALTITUDE PACKING + +#define MAVLINK_MSG_ID_SET_ALTITUDE 65 + +typedef struct __mavlink_set_altitude_t +{ + uint8_t target; ///< The system setting the altitude + uint32_t mode; ///< The new altitude in meters +} 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, \ + { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_altitude_t, target) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT32_T, 0, 1, offsetof(mavlink_set_altitude_t, mode) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target); + _mav_put_uint32_t(buf, 1, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); +#else + mavlink_set_altitude_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, 5); +} + +/** + * @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_uint8_t(buf, 0, target); + _mav_put_uint32_t(buf, 1, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); +#else + mavlink_set_altitude_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5); +} + +/** + * @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_uint8_t(buf, 0, target); + _mav_put_uint32_t(buf, 1, mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ALTITUDE, buf, 5); +#else + mavlink_set_altitude_t packet; + packet.target = target; + packet.mode = mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ALTITUDE, (const char *)&packet, 5); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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->target = mavlink_msg_set_altitude_get_target(msg); + set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); +#else + memcpy(set_altitude, _MAV_PAYLOAD(msg), 5); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_mode.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_mode.h new file mode 100644 index 000000000..8a3f20593 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_mode.h @@ -0,0 +1,166 @@ +// MESSAGE SET_MODE PACKING + +#define MAVLINK_MSG_ID_SET_MODE 11 + +typedef struct __mavlink_set_mode_t +{ + uint8_t target; ///< The system setting the mode + uint8_t mode; ///< The new mode +} mavlink_set_mode_t; + +#define MAVLINK_MSG_ID_SET_MODE_LEN 2 +#define MAVLINK_MSG_ID_11_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_SET_MODE { \ + "SET_MODE", \ + 2, \ + { { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_mode_t, target) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_mode_t, mode) }, \ + } \ +} + + +/** + * @brief Pack a set_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 mode The new mode + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, uint8_t 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, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); +#else + mavlink_set_mode_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MODE; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @brief Pack a set_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 mode The new mode + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t 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, mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 2); +#else + mavlink_set_mode_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MODE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @brief Encode a set_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_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mode_t* set_mode) +{ + return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target, set_mode->mode); +} + +/** + * @brief Send a set_mode message + * @param chan MAVLink channel to send the message + * + * @param target The system setting the mode + * @param mode The new mode + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t 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, mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, 2); +#else + mavlink_set_mode_t packet; + packet.target = target; + packet.mode = mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, 2); +#endif +} + +#endif + +// MESSAGE SET_MODE UNPACKING + + +/** + * @brief Get field target from set_mode message + * + * @return The system setting the mode + */ +static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field mode from set_mode message + * + * @return The new mode + */ +static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a set_mode message into a struct + * + * @param msg The message to decode + * @param set_mode C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mavlink_set_mode_t* set_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_mode->target = mavlink_msg_set_mode_get_target(msg); + set_mode->mode = mavlink_msg_set_mode_get_mode(msg); +#else + memcpy(set_mode, _MAV_PAYLOAD(msg), 2); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_nav_mode.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_nav_mode.h new file mode 100644 index 000000000..02efa26b1 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_nav_mode.h @@ -0,0 +1,166 @@ +// 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_NON_CONST(msg), buf, 2); +#else + mavlink_set_nav_mode_t packet; + packet.target = target; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @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_NON_CONST(msg), buf, 2); +#else + mavlink_set_nav_mode_t packet; + packet.target = target; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h new file mode 100644 index 000000000..6a9c01215 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -0,0 +1,254 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 56 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 +} mavlink_set_roll_pitch_yaw_speed_thrust_t; + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18 +#define MAVLINK_MSG_ID_56_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST { \ + "SET_ROLL_PITCH_YAW_SPEED_THRUST", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_component) }, \ + { "roll_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, roll_speed) }, \ + { "pitch_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, pitch_speed) }, \ + { "yaw_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, yaw_speed) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, thrust) }, \ + } \ +} + + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust 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 roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, roll_speed); + _mav_put_float(buf, 6, pitch_speed); + _mav_put_float(buf, 10, yaw_speed); + _mav_put_float(buf, 14, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust 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 roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_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 roll_speed,float pitch_speed,float yaw_speed,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, roll_speed); + _mav_put_float(buf, 6, pitch_speed); + _mav_put_float(buf, 10, yaw_speed); + _mav_put_float(buf, 14, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed_thrust 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_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + +/** + * @brief Send a set_roll_pitch_yaw_speed_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, roll_speed); + _mav_put_float(buf, 6, pitch_speed); + _mav_put_float(buf, 10, yaw_speed); + _mav_put_float(buf, 14, thrust); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, 18); +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, 18); +#endif +} + +#endif + +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING + + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 2); +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 6); +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 10); +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 14); +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg); + set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg); + set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg); + set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg); + set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg); + set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg); +#else + memcpy(set_roll_pitch_yaw_speed_thrust, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_roll_pitch_yaw_thrust.h new file mode 100644 index 000000000..dad4f657d --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -0,0 +1,254 @@ +// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 55 + +typedef struct __mavlink_set_roll_pitch_yaw_thrust_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 +} mavlink_set_roll_pitch_yaw_thrust_t; + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18 +#define MAVLINK_MSG_ID_55_LEN 18 + + + +#define MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST { \ + "SET_ROLL_PITCH_YAW_THRUST", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_component) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, yaw) }, \ + { "thrust", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, thrust) }, \ + } \ +} + + +/** + * @brief Pack a set_roll_pitch_yaw_thrust 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 roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, roll); + _mav_put_float(buf, 6, pitch); + _mav_put_float(buf, 10, yaw); + _mav_put_float(buf, 14, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_thrust_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + return mavlink_finalize_message(msg, system_id, component_id, 18); +} + +/** + * @brief Pack a set_roll_pitch_yaw_thrust 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 roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_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 roll,float pitch,float yaw,float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, roll); + _mav_put_float(buf, 6, pitch); + _mav_put_float(buf, 10, yaw); + _mav_put_float(buf, 14, thrust); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_thrust_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 18); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); +} + +/** + * @brief Encode a set_roll_pitch_yaw_thrust 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_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + +/** + * @brief Send a set_roll_pitch_yaw_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_float(buf, 2, roll); + _mav_put_float(buf, 6, pitch); + _mav_put_float(buf, 10, yaw); + _mav_put_float(buf, 14, thrust); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, 18); +#else + mavlink_set_roll_pitch_yaw_thrust_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, 18); +#endif +} + +#endif + +// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING + + +/** + * @brief Get field target_system from set_roll_pitch_yaw_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field roll from set_roll_pitch_yaw_thrust message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 2); +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw_thrust message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 6); +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw_thrust message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 10); +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 14); +} + +/** + * @brief Decode a set_roll_pitch_yaw_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg); + set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg); + set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg); + set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg); + set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg); + set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg); +#else + memcpy(set_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), 18); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_state_correction.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_state_correction.h new file mode 100644 index 000000000..ddd785a2a --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_state_correction.h @@ -0,0 +1,320 @@ +// MESSAGE STATE_CORRECTION PACKING + +#define MAVLINK_MSG_ID_STATE_CORRECTION 64 + +typedef struct __mavlink_state_correction_t +{ + float xErr; ///< x position error + float yErr; ///< y position error + float zErr; ///< z position error + float rollErr; ///< roll error (radians) + float pitchErr; ///< pitch error (radians) + float yawErr; ///< yaw error (radians) + float vxErr; ///< x velocity + float vyErr; ///< y velocity + float vzErr; ///< z velocity +} mavlink_state_correction_t; + +#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 +#define MAVLINK_MSG_ID_64_LEN 36 + + + +#define MAVLINK_MESSAGE_INFO_STATE_CORRECTION { \ + "STATE_CORRECTION", \ + 9, \ + { { "xErr", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_state_correction_t, xErr) }, \ + { "yErr", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_state_correction_t, yErr) }, \ + { "zErr", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_state_correction_t, zErr) }, \ + { "rollErr", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_state_correction_t, rollErr) }, \ + { "pitchErr", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_state_correction_t, pitchErr) }, \ + { "yawErr", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_state_correction_t, yawErr) }, \ + { "vxErr", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_state_correction_t, vxErr) }, \ + { "vyErr", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_state_correction_t, vyErr) }, \ + { "vzErr", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_state_correction_t, vzErr) }, \ + } \ +} + + +/** + * @brief Pack a state_correction 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 xErr x position error + * @param yErr y position error + * @param zErr z position error + * @param rollErr roll error (radians) + * @param pitchErr pitch error (radians) + * @param yawErr yaw error (radians) + * @param vxErr x velocity + * @param vyErr y velocity + * @param vzErr z velocity + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36); +#else + mavlink_state_correction_t packet; + packet.xErr = xErr; + packet.yErr = yErr; + packet.zErr = zErr; + packet.rollErr = rollErr; + packet.pitchErr = pitchErr; + packet.yawErr = yawErr; + packet.vxErr = vxErr; + packet.vyErr = vyErr; + packet.vzErr = vzErr; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + return mavlink_finalize_message(msg, system_id, component_id, 36); +} + +/** + * @brief Pack a state_correction 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 xErr x position error + * @param yErr y position error + * @param zErr z position error + * @param rollErr roll error (radians) + * @param pitchErr pitch error (radians) + * @param yawErr yaw error (radians) + * @param vxErr x velocity + * @param vyErr y velocity + * @param vzErr z velocity + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float xErr,float yErr,float zErr,float rollErr,float pitchErr,float yawErr,float vxErr,float vyErr,float vzErr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36); +#else + mavlink_state_correction_t packet; + packet.xErr = xErr; + packet.yErr = yErr; + packet.zErr = zErr; + packet.rollErr = rollErr; + packet.pitchErr = pitchErr; + packet.yawErr = yawErr; + packet.vxErr = vxErr; + packet.vyErr = vyErr; + packet.vzErr = vzErr; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36); +} + +/** + * @brief Encode a state_correction 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 state_correction C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_state_correction_t* state_correction) +{ + return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); +} + +/** + * @brief Send a state_correction message + * @param chan MAVLink channel to send the message + * + * @param xErr x position error + * @param yErr y position error + * @param zErr z position error + * @param rollErr roll error (radians) + * @param pitchErr pitch error (radians) + * @param yawErr yaw error (radians) + * @param vxErr x velocity + * @param vyErr y velocity + * @param vzErr z velocity + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, 36); +#else + mavlink_state_correction_t packet; + packet.xErr = xErr; + packet.yErr = yErr; + packet.zErr = zErr; + packet.rollErr = rollErr; + packet.pitchErr = pitchErr; + packet.yawErr = yawErr; + packet.vxErr = vxErr; + packet.vyErr = vyErr; + packet.vzErr = vzErr; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, 36); +#endif +} + +#endif + +// MESSAGE STATE_CORRECTION UNPACKING + + +/** + * @brief Get field xErr from state_correction message + * + * @return x position error + */ +static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field yErr from state_correction message + * + * @return y position error + */ +static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field zErr from state_correction message + * + * @return z position error + */ +static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field rollErr from state_correction message + * + * @return roll error (radians) + */ +static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field pitchErr from state_correction message + * + * @return pitch error (radians) + */ +static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field yawErr from state_correction message + * + * @return yaw error (radians) + */ +static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vxErr from state_correction message + * + * @return x velocity + */ +static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field vyErr from state_correction message + * + * @return y velocity + */ +static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vzErr from state_correction message + * + * @return z velocity + */ +static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Decode a state_correction message into a struct + * + * @param msg The message to decode + * @param state_correction C-struct to decode the message contents into + */ +static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* msg, mavlink_state_correction_t* state_correction) +{ +#if MAVLINK_NEED_BYTE_SWAP + state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg); + state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg); + state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg); + state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg); + state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg); + state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg); + state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg); + state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); + state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); +#else + memcpy(state_correction, _MAV_PAYLOAD(msg), 36); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_statustext.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_statustext.h new file mode 100644 index 000000000..c65f629e9 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_statustext.h @@ -0,0 +1,160 @@ +// MESSAGE STATUSTEXT PACKING + +#define MAVLINK_MSG_ID_STATUSTEXT 254 + +typedef struct __mavlink_statustext_t +{ + uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault + int8_t text[50]; ///< Status text message, without null termination character +} mavlink_statustext_t; + +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_ID_254_LEN 51 + +#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 + +#define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ + "STATUSTEXT", \ + 2, \ + { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ + { "text", NULL, MAVLINK_TYPE_INT8_T, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + } \ +} + + +/** + * @brief Pack a statustext 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 severity Severity of status, 0 = info message, 255 = critical fault + * @param text Status text message, without null termination character + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t severity, const int8_t *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[51]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_int8_t_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(int8_t)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 51); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; + return mavlink_finalize_message(msg, system_id, component_id, 51); +} + +/** + * @brief Pack a statustext 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 severity Severity of status, 0 = info message, 255 = critical fault + * @param text Status text message, without null termination character + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t severity,const int8_t *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[51]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_int8_t_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(int8_t)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 51); +#endif + + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51); +} + +/** + * @brief Encode a statustext 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 statustext C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext) +{ + return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); +} + +/** + * @brief Send a statustext message + * @param chan MAVLink channel to send the message + * + * @param severity Severity of status, 0 = info message, 255 = critical fault + * @param text Status text message, without null termination character + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t *text) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[51]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_int8_t_array(buf, 1, text, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + mav_array_memcpy(packet.text, text, sizeof(int8_t)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, 51); +#endif +} + +#endif + +// MESSAGE STATUSTEXT UNPACKING + + +/** + * @brief Get field severity from statustext message + * + * @return Severity of status, 0 = info message, 255 = critical fault + */ +static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field text from statustext message + * + * @return Status text message, without null termination character + */ +static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t *text) +{ + return _MAV_RETURN_int8_t_array(msg, text, 50, 1); +} + +/** + * @brief Decode a statustext message into a struct + * + * @param msg The message to decode + * @param statustext C-struct to decode the message contents into + */ +static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, mavlink_statustext_t* statustext) +{ +#if MAVLINK_NEED_BYTE_SWAP + statustext->severity = mavlink_msg_statustext_get_severity(msg); + mavlink_msg_statustext_get_text(msg, statustext->text); +#else + memcpy(statustext, _MAV_PAYLOAD(msg), 51); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_sys_status.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_sys_status.h new file mode 100644 index 000000000..1217ea60e --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_sys_status.h @@ -0,0 +1,276 @@ +// MESSAGE SYS_STATUS PACKING + +#define MAVLINK_MSG_ID_SYS_STATUS 34 + +typedef struct __mavlink_sys_status_t +{ + uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + uint8_t status; ///< System status flag, see MAV_STATUS ENUM + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) + uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) + uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) +} mavlink_sys_status_t; + +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 11 +#define MAVLINK_MSG_ID_34_LEN 11 + + + +#define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ + "SYS_STATUS", \ + 7, \ + { { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_sys_status_t, mode) }, \ + { "nav_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_sys_status_t, nav_mode) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_sys_status_t, status) }, \ + { "load", NULL, MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_sys_status_t, load) }, \ + { "vbat", NULL, MAVLINK_TYPE_UINT16_T, 0, 5, offsetof(mavlink_sys_status_t, vbat) }, \ + { "battery_remaining", NULL, MAVLINK_TYPE_UINT16_T, 0, 7, offsetof(mavlink_sys_status_t, battery_remaining) }, \ + { "packet_drop", NULL, MAVLINK_TYPE_UINT16_T, 0, 9, offsetof(mavlink_sys_status_t, packet_drop) }, \ + } \ +} + + +/** + * @brief Pack a sys_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 mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM + * @param status System status flag, see MAV_STATUS ENUM + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) + * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[11]; + _mav_put_uint8_t(buf, 0, mode); + _mav_put_uint8_t(buf, 1, nav_mode); + _mav_put_uint8_t(buf, 2, status); + _mav_put_uint16_t(buf, 3, load); + _mav_put_uint16_t(buf, 5, vbat); + _mav_put_uint16_t(buf, 7, battery_remaining); + _mav_put_uint16_t(buf, 9, packet_drop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 11); +#else + mavlink_sys_status_t packet; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.status = status; + packet.load = load; + packet.vbat = vbat; + packet.battery_remaining = battery_remaining; + packet.packet_drop = packet_drop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 11); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 11); +} + +/** + * @brief Pack a sys_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 mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM + * @param status System status flag, see MAV_STATUS ENUM + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) + * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t mode,uint8_t nav_mode,uint8_t status,uint16_t load,uint16_t vbat,uint16_t battery_remaining,uint16_t packet_drop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[11]; + _mav_put_uint8_t(buf, 0, mode); + _mav_put_uint8_t(buf, 1, nav_mode); + _mav_put_uint8_t(buf, 2, status); + _mav_put_uint16_t(buf, 3, load); + _mav_put_uint16_t(buf, 5, vbat); + _mav_put_uint16_t(buf, 7, battery_remaining); + _mav_put_uint16_t(buf, 9, packet_drop); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 11); +#else + mavlink_sys_status_t packet; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.status = status; + packet.load = load; + packet.vbat = vbat; + packet.battery_remaining = battery_remaining; + packet.packet_drop = packet_drop; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 11); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11); +} + +/** + * @brief Encode a sys_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 sys_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) +{ + return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->mode, sys_status->nav_mode, sys_status->status, sys_status->load, sys_status->vbat, sys_status->battery_remaining, sys_status->packet_drop); +} + +/** + * @brief Send a sys_status message + * @param chan MAVLink channel to send the message + * + * @param mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM + * @param status System status flag, see MAV_STATUS ENUM + * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + * @param vbat Battery voltage, in millivolts (1 = 1 millivolt) + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) + * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[11]; + _mav_put_uint8_t(buf, 0, mode); + _mav_put_uint8_t(buf, 1, nav_mode); + _mav_put_uint8_t(buf, 2, status); + _mav_put_uint16_t(buf, 3, load); + _mav_put_uint16_t(buf, 5, vbat); + _mav_put_uint16_t(buf, 7, battery_remaining); + _mav_put_uint16_t(buf, 9, packet_drop); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, 11); +#else + mavlink_sys_status_t packet; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.status = status; + packet.load = load; + packet.vbat = vbat; + packet.battery_remaining = battery_remaining; + packet.packet_drop = packet_drop; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, 11); +#endif +} + +#endif + +// MESSAGE SYS_STATUS UNPACKING + + +/** + * @brief Get field mode from sys_status message + * + * @return System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + */ +static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field nav_mode from sys_status message + * + * @return Navigation mode, see MAV_NAV_MODE ENUM + */ +static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field status from sys_status message + * + * @return System status flag, see MAV_STATUS ENUM + */ +static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field load from sys_status message + * + * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + */ +static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 3); +} + +/** + * @brief Get field vbat from sys_status message + * + * @return Battery voltage, in millivolts (1 = 1 millivolt) + */ +static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 5); +} + +/** + * @brief Get field battery_remaining from sys_status message + * + * @return Remaining battery energy: (0%: 0, 100%: 1000) + */ +static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 7); +} + +/** + * @brief Get field packet_drop from sys_status message + * + * @return Dropped packets (packets that were corrupted on reception on the MAV) + */ +static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 9); +} + +/** + * @brief Decode a sys_status message into a struct + * + * @param msg The message to decode + * @param sys_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + sys_status->mode = mavlink_msg_sys_status_get_mode(msg); + sys_status->nav_mode = mavlink_msg_sys_status_get_nav_mode(msg); + sys_status->status = mavlink_msg_sys_status_get_status(msg); + sys_status->load = mavlink_msg_sys_status_get_load(msg); + sys_status->vbat = mavlink_msg_sys_status_get_vbat(msg); + sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); + sys_status->packet_drop = mavlink_msg_sys_status_get_packet_drop(msg); +#else + memcpy(sys_status, _MAV_PAYLOAD(msg), 11); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_system_time.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_system_time.h new file mode 100644 index 000000000..362586a70 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_system_time.h @@ -0,0 +1,144 @@ +// MESSAGE SYSTEM_TIME PACKING + +#define MAVLINK_MSG_ID_SYSTEM_TIME 2 + +typedef struct __mavlink_system_time_t +{ + uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. +} mavlink_system_time_t; + +#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 8 +#define MAVLINK_MSG_ID_2_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME { \ + "SYSTEM_TIME", \ + 1, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_usec) }, \ + } \ +} + + +/** + * @brief Pack a system_time 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 of the master clock in microseconds since UNIX epoch. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint64_t(buf, 0, time_usec); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 8); +#else + mavlink_system_time_t packet; + packet.time_usec = time_usec; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + return mavlink_finalize_message(msg, system_id, component_id, 8); +} + +/** + * @brief Pack a system_time 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 of the master clock in microseconds since UNIX epoch. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint64_t(buf, 0, time_usec); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 8); +#else + mavlink_system_time_t packet; + packet.time_usec = time_usec; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); +} + +/** + * @brief Encode a system_time 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 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_t* system_time) +{ + return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_usec); +} + +/** + * @brief Send a system_time message + * @param chan MAVLink channel to send the message + * + * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint64_t(buf, 0, time_usec); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, 8); +#else + mavlink_system_time_t packet; + packet.time_usec = time_usec; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, 8); +#endif +} + +#endif + +// MESSAGE SYSTEM_TIME UNPACKING + + +/** + * @brief Get field time_usec from system_time message + * + * @return Timestamp of the master clock in microseconds since UNIX epoch. + */ +static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Decode a system_time message into a struct + * + * @param msg The message to decode + * @param system_time C-struct to decode the message contents into + */ +static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, mavlink_system_time_t* system_time) +{ +#if MAVLINK_NEED_BYTE_SWAP + system_time->time_usec = mavlink_msg_system_time_get_time_usec(msg); +#else + memcpy(system_time, _MAV_PAYLOAD(msg), 8); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_system_time_utc.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_system_time_utc.h new file mode 100644 index 000000000..5688435d5 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_system_time_utc.h @@ -0,0 +1,166 @@ +// 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_NON_CONST(msg), buf, 8); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + return mavlink_finalize_message(msg, system_id, component_id, 8); +} + +/** + * @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_NON_CONST(msg), buf, 8); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_vfr_hud.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_vfr_hud.h new file mode 100644 index 000000000..0f92de775 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_vfr_hud.h @@ -0,0 +1,254 @@ +// MESSAGE VFR_HUD PACKING + +#define MAVLINK_MSG_ID_VFR_HUD 74 + +typedef struct __mavlink_vfr_hud_t +{ + float airspeed; ///< Current airspeed in m/s + float groundspeed; ///< Current ground speed in m/s + int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 + float alt; ///< Current altitude (MSL), in meters + float climb; ///< Current climb rate in meters/second +} mavlink_vfr_hud_t; + +#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 +#define MAVLINK_MSG_ID_74_LEN 20 + + + +#define MAVLINK_MESSAGE_INFO_VFR_HUD { \ + "VFR_HUD", \ + 6, \ + { { "airspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_vfr_hud_t, airspeed) }, \ + { "groundspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_vfr_hud_t, groundspeed) }, \ + { "heading", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_vfr_hud_t, heading) }, \ + { "throttle", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_vfr_hud_t, throttle) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vfr_hud_t, alt) }, \ + { "climb", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vfr_hud_t, climb) }, \ + } \ +} + + +/** + * @brief Pack a vfr_hud message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_int16_t(buf, 8, heading); + _mav_put_uint16_t(buf, 10, throttle); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, climb); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.heading = heading; + packet.throttle = throttle; + packet.alt = alt; + packet.climb = climb; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); +#endif + + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; + return mavlink_finalize_message(msg, system_id, component_id, 20); +} + +/** + * @brief Pack a vfr_hud 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 airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_int16_t(buf, 8, heading); + _mav_put_uint16_t(buf, 10, throttle); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, climb); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.heading = heading; + packet.throttle = throttle; + packet.alt = alt; + packet.climb = climb; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); +#endif + + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20); +} + +/** + * @brief Encode a vfr_hud struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param vfr_hud C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vfr_hud_t* vfr_hud) +{ + return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); +} + +/** + * @brief Send a vfr_hud message + * @param chan MAVLink channel to send the message + * + * @param airspeed Current airspeed in m/s + * @param groundspeed Current ground speed in m/s + * @param heading Current heading in degrees, in compass units (0..360, 0=north) + * @param throttle Current throttle setting in integer percent, 0 to 100 + * @param alt Current altitude (MSL), in meters + * @param climb Current climb rate in meters/second + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_int16_t(buf, 8, heading); + _mav_put_uint16_t(buf, 10, throttle); + _mav_put_float(buf, 12, alt); + _mav_put_float(buf, 16, climb); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, 20); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.heading = heading; + packet.throttle = throttle; + packet.alt = alt; + packet.climb = climb; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, 20); +#endif +} + +#endif + +// MESSAGE VFR_HUD UNPACKING + + +/** + * @brief Get field airspeed from vfr_hud message + * + * @return Current airspeed in m/s + */ +static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field groundspeed from vfr_hud message + * + * @return Current ground speed in m/s + */ +static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field heading from vfr_hud message + * + * @return Current heading in degrees, in compass units (0..360, 0=north) + */ +static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 8); +} + +/** + * @brief Get field throttle from vfr_hud message + * + * @return Current throttle setting in integer percent, 0 to 100 + */ +static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field alt from vfr_hud message + * + * @return Current altitude (MSL), in meters + */ +static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field climb from vfr_hud message + * + * @return Current climb rate in meters/second + */ +static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a vfr_hud message into a struct + * + * @param msg The message to decode + * @param vfr_hud C-struct to decode the message contents into + */ +static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavlink_vfr_hud_t* vfr_hud) +{ +#if MAVLINK_NEED_BYTE_SWAP + vfr_hud->airspeed = mavlink_msg_vfr_hud_get_airspeed(msg); + vfr_hud->groundspeed = mavlink_msg_vfr_hud_get_groundspeed(msg); + vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); + vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); + vfr_hud->alt = mavlink_msg_vfr_hud_get_alt(msg); + vfr_hud->climb = mavlink_msg_vfr_hud_get_climb(msg); +#else + memcpy(vfr_hud, _MAV_PAYLOAD(msg), 20); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint.h new file mode 100644 index 000000000..d9b21e3c9 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint.h @@ -0,0 +1,430 @@ +// MESSAGE WAYPOINT PACKING + +#define MAVLINK_MSG_ID_WAYPOINT 39 + +typedef struct __mavlink_waypoint_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint16_t seq; ///< Sequence + 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 + 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 +} mavlink_waypoint_t; + +#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 +#define MAVLINK_MSG_ID_39_LEN 36 + + + +#define MAVLINK_MESSAGE_INFO_WAYPOINT { \ + "WAYPOINT", \ + 14, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_t, target_component) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_t, seq) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_waypoint_t, frame) }, \ + { "command", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_waypoint_t, command) }, \ + { "current", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_waypoint_t, current) }, \ + { "autocontinue", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_waypoint_t, autocontinue) }, \ + { "param1", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_waypoint_t, param1) }, \ + { "param2", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_waypoint_t, param2) }, \ + { "param3", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_waypoint_t, param3) }, \ + { "param4", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_waypoint_t, param4) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_waypoint_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_waypoint_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_waypoint_t, z) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + _mav_put_uint8_t(buf, 4, frame); + _mav_put_uint8_t(buf, 5, command); + _mav_put_uint8_t(buf, 6, current); + _mav_put_uint8_t(buf, 7, autocontinue); + _mav_put_float(buf, 8, param1); + _mav_put_float(buf, 12, param2); + _mav_put_float(buf, 16, param3); + _mav_put_float(buf, 20, param4); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36); +#else + mavlink_waypoint_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + packet.frame = frame; + packet.command = command; + packet.current = current; + packet.autocontinue = autocontinue; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; + return mavlink_finalize_message(msg, system_id, component_id, 36); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + _mav_put_uint8_t(buf, 4, frame); + _mav_put_uint8_t(buf, 5, command); + _mav_put_uint8_t(buf, 6, current); + _mav_put_uint8_t(buf, 7, autocontinue); + _mav_put_float(buf, 8, param1); + _mav_put_float(buf, 12, param2); + _mav_put_float(buf, 16, param3); + _mav_put_float(buf, 20, param4); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 36); +#else + mavlink_waypoint_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + packet.frame = frame; + packet.command = command; + packet.current = current; + packet.autocontinue = autocontinue; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + _mav_put_uint8_t(buf, 4, frame); + _mav_put_uint8_t(buf, 5, command); + _mav_put_uint8_t(buf, 6, current); + _mav_put_uint8_t(buf, 7, autocontinue); + _mav_put_float(buf, 8, param1); + _mav_put_float(buf, 12, param2); + _mav_put_float(buf, 16, param3); + _mav_put_float(buf, 20, param4); + _mav_put_float(buf, 24, x); + _mav_put_float(buf, 28, y); + _mav_put_float(buf, 32, z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, buf, 36); +#else + mavlink_waypoint_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + packet.frame = frame; + packet.command = command; + packet.current = current; + packet.autocontinue = autocontinue; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, (const char *)&packet, 36); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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, 4); +} + +/** + * @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, 5); +} + +/** + * @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, 6); +} + +/** + * @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, 7); +} + +/** + * @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, 8); +} + +/** + * @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, 12); +} + +/** + * @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, 16); +} + +/** + * @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, 20); +} + +/** + * @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, 24); +} + +/** + * @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, 28); +} + +/** + * @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, 32); +} + +/** + * @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->target_system = mavlink_msg_waypoint_get_target_system(msg); + waypoint->target_component = mavlink_msg_waypoint_get_target_component(msg); + waypoint->seq = mavlink_msg_waypoint_get_seq(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); + 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); +#else + memcpy(waypoint, _MAV_PAYLOAD(msg), 36); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_ack.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_ack.h new file mode 100644 index 000000000..ddba2adf4 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_ack.h @@ -0,0 +1,188 @@ +// 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_NON_CONST(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_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + return mavlink_finalize_message(msg, system_id, component_id, 3); +} + +/** + * @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_NON_CONST(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_NON_CONST(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_clear_all.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_clear_all.h new file mode 100644 index 000000000..8eab44b26 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_clear_all.h @@ -0,0 +1,166 @@ +// 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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_count.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_count.h new file mode 100644 index 000000000..af1dd382b --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_count.h @@ -0,0 +1,188 @@ +// MESSAGE WAYPOINT_COUNT PACKING + +#define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 + +typedef struct __mavlink_waypoint_count_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint16_t count; ///< Number of Waypoints in the Sequence +} 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, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_count_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_count_t, target_component) }, \ + { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_count_t, count) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); +#else + mavlink_waypoint_count_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.count = count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + return mavlink_finalize_message(msg, system_id, component_id, 4); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); +#else + mavlink_waypoint_count_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.count = count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, count); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, buf, 4); +#else + mavlink_waypoint_count_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.count = count; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, (const char *)&packet, 4); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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->target_system = mavlink_msg_waypoint_count_get_target_system(msg); + waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); + waypoint_count->count = mavlink_msg_waypoint_count_get_count(msg); +#else + memcpy(waypoint_count, _MAV_PAYLOAD(msg), 4); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_current.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_current.h new file mode 100644 index 000000000..a7e4557d6 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_current.h @@ -0,0 +1,144 @@ +// 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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @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); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, (const char *)&packet, 2); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_reached.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_reached.h new file mode 100644 index 000000000..d28dce149 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_reached.h @@ -0,0 +1,144 @@ +// 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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @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); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, (const char *)&packet, 2); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_request.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_request.h new file mode 100644 index 000000000..38167c080 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_request.h @@ -0,0 +1,188 @@ +// MESSAGE WAYPOINT_REQUEST PACKING + +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 + +typedef struct __mavlink_waypoint_request_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint16_t seq; ///< Sequence +} 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, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_request_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_request_t, target_component) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_request_t, seq) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); +#else + mavlink_waypoint_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + return mavlink_finalize_message(msg, system_id, component_id, 4); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); +#else + mavlink_waypoint_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, buf, 4); +#else + mavlink_waypoint_request_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, (const char *)&packet, 4); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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->target_system = mavlink_msg_waypoint_request_get_target_system(msg); + waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); + waypoint_request->seq = mavlink_msg_waypoint_request_get_seq(msg); +#else + memcpy(waypoint_request, _MAV_PAYLOAD(msg), 4); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_request_list.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_request_list.h new file mode 100644 index 000000000..ba21fbc82 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_request_list.h @@ -0,0 +1,166 @@ +// 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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + return mavlink_finalize_message(msg, system_id, component_id, 2); +} + +/** + * @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_NON_CONST(msg), buf, 2); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); +} + +/** + * @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); +#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); +#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/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_set_current.h b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_set_current.h new file mode 100644 index 000000000..f1ba68b45 --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/mavlink_msg_waypoint_set_current.h @@ -0,0 +1,188 @@ +// MESSAGE WAYPOINT_SET_CURRENT PACKING + +#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 + +typedef struct __mavlink_waypoint_set_current_t +{ + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint16_t seq; ///< Sequence +} 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, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_set_current_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_set_current_t, target_component) }, \ + { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_set_current_t, seq) }, \ + } \ +} + + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); +#else + mavlink_waypoint_set_current_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + return mavlink_finalize_message(msg, system_id, component_id, 4); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); +#else + mavlink_waypoint_set_current_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); +} + +/** + * @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_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint16_t(buf, 2, seq); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, buf, 4); +#else + mavlink_waypoint_set_current_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.seq = seq; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, (const char *)&packet, 4); +#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, 0); +} + +/** + * @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, 1); +} + +/** + * @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, 2); +} + +/** + * @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->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); + waypoint_set_current->seq = mavlink_msg_waypoint_set_current_get_seq(msg); +#else + memcpy(waypoint_set_current, _MAV_PAYLOAD(msg), 4); +#endif +} diff --git a/mavlink/include/mavlink/v0.9/common/testsuite.h b/mavlink/include/mavlink/v0.9/common/testsuite.h new file mode 100644 index 000000000..b3a94e96e --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/testsuite.h @@ -0,0 +1,3700 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from common.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef COMMON_TESTSUITE_H +#define COMMON_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + + mavlink_test_common(system_id, component_id, last_msg); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_heartbeat_t packet_in = { + 5, + 72, + 2, + }; + mavlink_heartbeat_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.type = packet_in.type; + packet1.autopilot = packet_in.autopilot; + packet1.mavlink_version = packet_in.mavlink_version; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_heartbeat_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_send(MAVLINK_COMM_1 , packet1.type , packet1.autopilot ); + mavlink_msg_heartbeat_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_boot(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_boot_t packet_in = { + 963497464, + }; + mavlink_boot_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.version = packet_in.version; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_boot_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_boot_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_boot_pack(system_id, component_id, &msg , packet1.version ); + mavlink_msg_boot_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_boot_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.version ); + mavlink_msg_boot_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_boot_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_boot_send(MAVLINK_COMM_1 , packet1.version ); + mavlink_msg_boot_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_system_time(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_system_time_t packet_in = { + 93372036854775807ULL, + }; + mavlink_system_time_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_system_time_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_pack(system_id, component_id, &msg , packet1.time_usec ); + mavlink_msg_system_time_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec ); + mavlink_msg_system_time_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_system_time_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_send(MAVLINK_COMM_1 , packet1.time_usec ); + mavlink_msg_system_time_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_ping(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ping_t packet_in = { + 963497464, + 17, + 84, + 93372036854776185ULL, + }; + mavlink_ping_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.seq = packet_in.seq; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.time = packet_in.time; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ping_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ping_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ping_pack(system_id, component_id, &msg , packet1.seq , packet1.target_system , packet1.target_component , packet1.time ); + mavlink_msg_ping_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ping_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seq , packet1.target_system , packet1.target_component , packet1.time ); + mavlink_msg_ping_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_ping_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ping_send(MAVLINK_COMM_1 , packet1.seq , packet1.target_system , packet1.target_component , packet1.time ); + mavlink_msg_ping_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_system_time_utc(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_system_time_utc_t packet_in = { + 963497464, + 963497672, + }; + mavlink_system_time_utc_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.utc_date = packet_in.utc_date; + packet1.utc_time = packet_in.utc_time; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_utc_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_system_time_utc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_utc_pack(system_id, component_id, &msg , packet1.utc_date , packet1.utc_time ); + mavlink_msg_system_time_utc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_utc_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.utc_date , packet1.utc_time ); + mavlink_msg_system_time_utc_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_system_time_utc_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_system_time_utc_send(MAVLINK_COMM_1 , packet1.utc_date , packet1.utc_time ); + mavlink_msg_system_time_utc_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_change_operator_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_change_operator_control_t packet_in = { + 5, + 72, + 139, + "DEFGHIJKLMNOPQRSTUVWXYZA", + }; + mavlink_change_operator_control_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.control_request = packet_in.control_request; + packet1.version = packet_in.version; + + mav_array_memcpy(packet1.passkey, packet_in.passkey, sizeof(char)*25); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_change_operator_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey ); + mavlink_msg_change_operator_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey ); + mavlink_msg_change_operator_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_change_operator_control_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_send(MAVLINK_COMM_1 , packet1.target_system , packet1.control_request , packet1.version , packet1.passkey ); + mavlink_msg_change_operator_control_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_change_operator_control_ack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_change_operator_control_ack_t packet_in = { + 5, + 72, + 139, + }; + mavlink_change_operator_control_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.gcs_system_id = packet_in.gcs_system_id; + packet1.control_request = packet_in.control_request; + packet1.ack = packet_in.ack; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_change_operator_control_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_ack_pack(system_id, component_id, &msg , packet1.gcs_system_id , packet1.control_request , packet1.ack ); + mavlink_msg_change_operator_control_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.gcs_system_id , packet1.control_request , packet1.ack ); + mavlink_msg_change_operator_control_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_change_operator_control_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_change_operator_control_ack_send(MAVLINK_COMM_1 , packet1.gcs_system_id , packet1.control_request , packet1.ack ); + mavlink_msg_change_operator_control_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_auth_key(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_auth_key_t packet_in = { + "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDE", + }; + mavlink_auth_key_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + + mav_array_memcpy(packet1.key, packet_in.key, sizeof(char)*32); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_auth_key_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_auth_key_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_auth_key_pack(system_id, component_id, &msg , packet1.key ); + mavlink_msg_auth_key_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_auth_key_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.key ); + mavlink_msg_auth_key_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_auth_key_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_auth_key_send(MAVLINK_COMM_1 , packet1.key ); + mavlink_msg_auth_key_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_action_ack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_action_ack_t packet_in = { + 5, + 72, + }; + mavlink_action_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.action = packet_in.action; + packet1.result = packet_in.result; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_action_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_ack_pack(system_id, component_id, &msg , packet1.action , packet1.result ); + mavlink_msg_action_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.action , packet1.result ); + mavlink_msg_action_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_action_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_ack_send(MAVLINK_COMM_1 , packet1.action , packet1.result ); + mavlink_msg_action_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_action(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_action_t packet_in = { + 5, + 72, + 139, + }; + mavlink_action_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target = packet_in.target; + packet1.target_component = packet_in.target_component; + packet1.action = packet_in.action; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_action_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_pack(system_id, component_id, &msg , packet1.target , packet1.target_component , packet1.action ); + mavlink_msg_action_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.target_component , packet1.action ); + mavlink_msg_action_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_action_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_action_send(MAVLINK_COMM_1 , packet1.target , packet1.target_component , packet1.action ); + mavlink_msg_action_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_set_mode(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_mode_t packet_in = { + 5, + 72, + }; + mavlink_set_mode_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target = packet_in.target; + packet1.mode = packet_in.mode; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_mode_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_mode_pack(system_id, component_id, &msg , packet1.target , packet1.mode ); + mavlink_msg_set_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_mode_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.mode ); + mavlink_msg_set_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_set_mode_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_mode_send(MAVLINK_COMM_1 , packet1.target , packet1.mode ); + mavlink_msg_set_mode_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_set_nav_mode(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_nav_mode_t packet_in = { + 5, + 72, + }; + mavlink_set_nav_mode_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target = packet_in.target; + packet1.nav_mode = packet_in.nav_mode; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_nav_mode_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_nav_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_nav_mode_pack(system_id, component_id, &msg , packet1.target , packet1.nav_mode ); + mavlink_msg_set_nav_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_nav_mode_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.nav_mode ); + mavlink_msg_set_nav_mode_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_set_nav_mode_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_nav_mode_send(MAVLINK_COMM_1 , packet1.target , packet1.nav_mode ); + mavlink_msg_set_nav_mode_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_param_request_read(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_request_read_t packet_in = { + 5, + 72, + { 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153 }, + 18119, + }; + mavlink_param_request_read_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.param_index = packet_in.param_index; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(int8_t)*15); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_read_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_read_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_read_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_param_request_read_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_read_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_request_read_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_param_request_list(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_request_list_t packet_in = { + 5, + 72, + }; + mavlink_param_request_list_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_list_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_param_request_list_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_param_request_list_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_param_value(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_value_t packet_in = { + { 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19 }, + 122.0, + 18223, + 18327, + }; + mavlink_param_value_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_value = packet_in.param_value; + packet1.param_count = packet_in.param_count; + packet1.param_index = packet_in.param_index; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(int8_t)*15); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_value_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_value_pack(system_id, component_id, &msg , packet1.param_id , packet1.param_value , packet1.param_count , packet1.param_index ); + mavlink_msg_param_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_value_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.param_id , packet1.param_value , packet1.param_count , packet1.param_index ); + mavlink_msg_param_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_param_value_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_value_send(MAVLINK_COMM_1 , packet1.param_id , packet1.param_value , packet1.param_count , packet1.param_index ); + mavlink_msg_param_value_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_param_set(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_set_t packet_in = { + 5, + 72, + { 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153 }, + 136.0, + }; + mavlink_param_set_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.param_value = packet_in.param_value; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(int8_t)*15); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_set_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value ); + mavlink_msg_param_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value ); + mavlink_msg_param_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_param_set_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_set_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value ); + mavlink_msg_param_set_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_gps_raw_int(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_raw_int_t packet_in = { + 93372036854775807ULL, + 29, + 963497932, + 963498140, + 963498348, + 164.0, + 192.0, + 220.0, + 248.0, + }; + mavlink_gps_raw_int_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.fix_type = packet_in.fix_type; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.eph = packet_in.eph; + packet1.epv = packet_in.epv; + packet1.v = packet_in.v; + packet1.hdg = packet_in.hdg; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_int_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_raw_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_int_pack(system_id, component_id, &msg , packet1.usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.v , packet1.hdg ); + mavlink_msg_gps_raw_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.v , packet1.hdg ); + mavlink_msg_gps_raw_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_gps_raw_int_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_int_send(MAVLINK_COMM_1 , packet1.usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.v , packet1.hdg ); + mavlink_msg_gps_raw_int_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_scaled_imu(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_scaled_imu_t packet_in = { + 93372036854775807ULL, + 17651, + 17755, + 17859, + 17963, + 18067, + 18171, + 18275, + 18379, + 18483, + }; + mavlink_scaled_imu_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.xacc = packet_in.xacc; + packet1.yacc = packet_in.yacc; + packet1.zacc = packet_in.zacc; + packet1.xgyro = packet_in.xgyro; + packet1.ygyro = packet_in.ygyro; + packet1.zgyro = packet_in.zgyro; + packet1.xmag = packet_in.xmag; + packet1.ymag = packet_in.ymag; + packet1.zmag = packet_in.zmag; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_scaled_imu_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu_pack(system_id, component_id, &msg , packet1.usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_scaled_imu_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_imu_send(MAVLINK_COMM_1 , packet1.usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_gps_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_status_t packet_in = { + 5, + { 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91 }, + { 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151 }, + { 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 }, + { 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }, + { 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75 }, + }; + mavlink_gps_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.satellites_visible = packet_in.satellites_visible; + + mav_array_memcpy(packet1.satellite_prn, packet_in.satellite_prn, sizeof(int8_t)*20); + mav_array_memcpy(packet1.satellite_used, packet_in.satellite_used, sizeof(int8_t)*20); + mav_array_memcpy(packet1.satellite_elevation, packet_in.satellite_elevation, sizeof(int8_t)*20); + mav_array_memcpy(packet1.satellite_azimuth, packet_in.satellite_azimuth, sizeof(int8_t)*20); + mav_array_memcpy(packet1.satellite_snr, packet_in.satellite_snr, sizeof(int8_t)*20); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_status_pack(system_id, component_id, &msg , packet1.satellites_visible , packet1.satellite_prn , packet1.satellite_used , packet1.satellite_elevation , packet1.satellite_azimuth , packet1.satellite_snr ); + mavlink_msg_gps_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.satellites_visible , packet1.satellite_prn , packet1.satellite_used , packet1.satellite_elevation , packet1.satellite_azimuth , packet1.satellite_snr ); + mavlink_msg_gps_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_gps_status_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_status_send(MAVLINK_COMM_1 , packet1.satellites_visible , packet1.satellite_prn , packet1.satellite_used , packet1.satellite_elevation , packet1.satellite_azimuth , packet1.satellite_snr ); + mavlink_msg_gps_status_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_raw_imu(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_raw_imu_t packet_in = { + 93372036854775807ULL, + 17651, + 17755, + 17859, + 17963, + 18067, + 18171, + 18275, + 18379, + 18483, + }; + mavlink_raw_imu_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.xacc = packet_in.xacc; + packet1.yacc = packet_in.yacc; + packet1.zacc = packet_in.zacc; + packet1.xgyro = packet_in.xgyro; + packet1.ygyro = packet_in.ygyro; + packet1.zgyro = packet_in.zgyro; + packet1.xmag = packet_in.xmag; + packet1.ymag = packet_in.ymag; + packet1.zmag = packet_in.zmag; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_imu_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_raw_imu_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_imu_pack(system_id, component_id, &msg , packet1.usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_raw_imu_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_raw_imu_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_raw_imu_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_imu_send(MAVLINK_COMM_1 , packet1.usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_raw_imu_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_raw_pressure(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_raw_pressure_t packet_in = { + 93372036854775807ULL, + 17651, + 17755, + 17859, + 17963, + }; + mavlink_raw_pressure_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.press_abs = packet_in.press_abs; + packet1.press_diff1 = packet_in.press_diff1; + packet1.press_diff2 = packet_in.press_diff2; + packet1.temperature = packet_in.temperature; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_pressure_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_raw_pressure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_pressure_pack(system_id, component_id, &msg , packet1.usec , packet1.press_abs , packet1.press_diff1 , packet1.press_diff2 , packet1.temperature ); + mavlink_msg_raw_pressure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_pressure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.press_abs , packet1.press_diff1 , packet1.press_diff2 , packet1.temperature ); + mavlink_msg_raw_pressure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_raw_pressure_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_pressure_send(MAVLINK_COMM_1 , packet1.usec , packet1.press_abs , packet1.press_diff1 , packet1.press_diff2 , packet1.temperature ); + mavlink_msg_raw_pressure_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_scaled_pressure(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_scaled_pressure_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 18067, + }; + mavlink_scaled_pressure_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.press_abs = packet_in.press_abs; + packet1.press_diff = packet_in.press_diff; + packet1.temperature = packet_in.temperature; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_pressure_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_scaled_pressure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_pressure_pack(system_id, component_id, &msg , packet1.usec , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_scaled_pressure_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_scaled_pressure_send(MAVLINK_COMM_1 , packet1.usec , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_attitude(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_attitude_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + }; + mavlink_attitude_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.rollspeed = packet_in.rollspeed; + packet1.pitchspeed = packet_in.pitchspeed; + packet1.yawspeed = packet_in.yawspeed; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_pack(system_id, component_id, &msg , packet1.usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_attitude_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_attitude_send(MAVLINK_COMM_1 , packet1.usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_local_position(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_local_position_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + }; + mavlink_local_position_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_local_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_local_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_local_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_local_position_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_local_position_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_global_position(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_global_position_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + }; + mavlink_global_position_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_pack(system_id, component_id, &msg , packet1.usec , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_global_position_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_send(MAVLINK_COMM_1 , packet1.usec , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_global_position_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_gps_raw(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_raw_t packet_in = { + 93372036854775807ULL, + 29, + 80.0, + 108.0, + 136.0, + 164.0, + 192.0, + 220.0, + 248.0, + }; + mavlink_gps_raw_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.fix_type = packet_in.fix_type; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.eph = packet_in.eph; + packet1.epv = packet_in.epv; + packet1.v = packet_in.v; + packet1.hdg = packet_in.hdg; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_pack(system_id, component_id, &msg , packet1.usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.v , packet1.hdg ); + mavlink_msg_gps_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.v , packet1.hdg ); + mavlink_msg_gps_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_gps_raw_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_raw_send(MAVLINK_COMM_1 , packet1.usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.v , packet1.hdg ); + mavlink_msg_gps_raw_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_sys_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_sys_status_t packet_in = { + 5, + 72, + 139, + 17391, + 17495, + 17599, + 17703, + }; + mavlink_sys_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mode = packet_in.mode; + packet1.nav_mode = packet_in.nav_mode; + packet1.status = packet_in.status; + packet1.load = packet_in.load; + packet1.vbat = packet_in.vbat; + packet1.battery_remaining = packet_in.battery_remaining; + packet1.packet_drop = packet_in.packet_drop; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sys_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_sys_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sys_status_pack(system_id, component_id, &msg , packet1.mode , packet1.nav_mode , packet1.status , packet1.load , packet1.vbat , packet1.battery_remaining , packet1.packet_drop ); + mavlink_msg_sys_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sys_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mode , packet1.nav_mode , packet1.status , packet1.load , packet1.vbat , packet1.battery_remaining , packet1.packet_drop ); + mavlink_msg_sys_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_sys_status_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sys_status_send(MAVLINK_COMM_1 , packet1.mode , packet1.nav_mode , packet1.status , packet1.load , packet1.vbat , packet1.battery_remaining , packet1.packet_drop ); + mavlink_msg_sys_status_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_rc_channels_raw(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_rc_channels_raw_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + 17859, + 17963, + 53, + }; + mavlink_rc_channels_raw_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.chan1_raw = packet_in.chan1_raw; + packet1.chan2_raw = packet_in.chan2_raw; + packet1.chan3_raw = packet_in.chan3_raw; + packet1.chan4_raw = packet_in.chan4_raw; + packet1.chan5_raw = packet_in.chan5_raw; + packet1.chan6_raw = packet_in.chan6_raw; + packet1.chan7_raw = packet_in.chan7_raw; + packet1.chan8_raw = packet_in.chan8_raw; + packet1.rssi = packet_in.rssi; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_raw_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rc_channels_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_raw_pack(system_id, component_id, &msg , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.rssi ); + mavlink_msg_rc_channels_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.rssi ); + mavlink_msg_rc_channels_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_rc_channels_raw_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_raw_send(MAVLINK_COMM_1 , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.rssi ); + mavlink_msg_rc_channels_raw_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_rc_channels_scaled(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_rc_channels_scaled_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + 17859, + 17963, + 53, + }; + mavlink_rc_channels_scaled_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.chan1_scaled = packet_in.chan1_scaled; + packet1.chan2_scaled = packet_in.chan2_scaled; + packet1.chan3_scaled = packet_in.chan3_scaled; + packet1.chan4_scaled = packet_in.chan4_scaled; + packet1.chan5_scaled = packet_in.chan5_scaled; + packet1.chan6_scaled = packet_in.chan6_scaled; + packet1.chan7_scaled = packet_in.chan7_scaled; + packet1.chan8_scaled = packet_in.chan8_scaled; + packet1.rssi = packet_in.rssi; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_scaled_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rc_channels_scaled_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_scaled_pack(system_id, component_id, &msg , packet1.chan1_scaled , packet1.chan2_scaled , packet1.chan3_scaled , packet1.chan4_scaled , packet1.chan5_scaled , packet1.chan6_scaled , packet1.chan7_scaled , packet1.chan8_scaled , packet1.rssi ); + mavlink_msg_rc_channels_scaled_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_scaled_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.chan1_scaled , packet1.chan2_scaled , packet1.chan3_scaled , packet1.chan4_scaled , packet1.chan5_scaled , packet1.chan6_scaled , packet1.chan7_scaled , packet1.chan8_scaled , packet1.rssi ); + mavlink_msg_rc_channels_scaled_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_rc_channels_scaled_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_scaled_send(MAVLINK_COMM_1 , packet1.chan1_scaled , packet1.chan2_scaled , packet1.chan3_scaled , packet1.chan4_scaled , packet1.chan5_scaled , packet1.chan6_scaled , packet1.chan7_scaled , packet1.chan8_scaled , packet1.rssi ); + mavlink_msg_rc_channels_scaled_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_servo_output_raw_t packet_in = { + 17235, + 17339, + 17443, + 17547, + 17651, + 17755, + 17859, + 17963, + }; + mavlink_servo_output_raw_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.servo1_raw = packet_in.servo1_raw; + packet1.servo2_raw = packet_in.servo2_raw; + packet1.servo3_raw = packet_in.servo3_raw; + packet1.servo4_raw = packet_in.servo4_raw; + packet1.servo5_raw = packet_in.servo5_raw; + packet1.servo6_raw = packet_in.servo6_raw; + packet1.servo7_raw = packet_in.servo7_raw; + packet1.servo8_raw = packet_in.servo8_raw; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_servo_output_raw_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_servo_output_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_servo_output_raw_pack(system_id, component_id, &msg , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_servo_output_raw_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_1 , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_t packet_in = { + 5, + 72, + 17339, + 17, + 84, + 151, + 218, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 241.0, + }; + mavlink_waypoint_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.seq = packet_in.seq; + packet1.frame = packet_in.frame; + packet1.command = packet_in.command; + packet1.current = packet_in.current; + packet1.autocontinue = packet_in.autocontinue; + packet1.param1 = packet_in.param1; + packet1.param2 = packet_in.param2; + packet1.param3 = packet_in.param3; + packet1.param4 = packet_in.param4; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_waypoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_waypoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_waypoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_request(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_request_t packet_in = { + 5, + 72, + 17339, + }; + mavlink_waypoint_request_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.seq = packet_in.seq; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_request_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_waypoint_request_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_waypoint_request_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_request_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_waypoint_request_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_set_current(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_set_current_t packet_in = { + 5, + 72, + 17339, + }; + mavlink_waypoint_set_current_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.seq = packet_in.seq; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_set_current_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_set_current_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_set_current_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_waypoint_set_current_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_set_current_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_waypoint_set_current_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_set_current_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_set_current_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_waypoint_set_current_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_current(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_current_t packet_in = { + 17235, + }; + mavlink_waypoint_current_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.seq = packet_in.seq; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_current_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_current_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_current_pack(system_id, component_id, &msg , packet1.seq ); + mavlink_msg_waypoint_current_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_current_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seq ); + mavlink_msg_waypoint_current_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_current_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_current_send(MAVLINK_COMM_1 , packet1.seq ); + mavlink_msg_waypoint_current_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_request_list(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_request_list_t packet_in = { + 5, + 72, + }; + mavlink_waypoint_request_list_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_list_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_waypoint_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_waypoint_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_request_list_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_waypoint_request_list_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_count(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_count_t packet_in = { + 5, + 72, + 17339, + }; + mavlink_waypoint_count_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.count = packet_in.count; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_count_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_count_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_count_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_waypoint_count_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_count_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_waypoint_count_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_count_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_count_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_waypoint_count_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_clear_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_clear_all_t packet_in = { + 5, + 72, + }; + mavlink_waypoint_clear_all_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_clear_all_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_clear_all_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_clear_all_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_waypoint_clear_all_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_clear_all_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_waypoint_clear_all_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_clear_all_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_clear_all_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_waypoint_clear_all_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_reached(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_reached_t packet_in = { + 17235, + }; + mavlink_waypoint_reached_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.seq = packet_in.seq; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_reached_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_reached_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_reached_pack(system_id, component_id, &msg , packet1.seq ); + mavlink_msg_waypoint_reached_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_reached_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.seq ); + mavlink_msg_waypoint_reached_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_reached_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_reached_send(MAVLINK_COMM_1 , packet1.seq ); + mavlink_msg_waypoint_reached_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_waypoint_ack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_waypoint_ack_t packet_in = { + 5, + 72, + 139, + }; + mavlink_waypoint_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.type = packet_in.type; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_waypoint_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_waypoint_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_waypoint_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_waypoint_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_waypoint_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_waypoint_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_gps_set_global_origin(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_set_global_origin_t packet_in = { + 5, + 72, + 963497568, + 963497776, + 963497984, + }; + mavlink_gps_set_global_origin_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.latitude = packet_in.latitude; + packet1.longitude = packet_in.longitude; + packet1.altitude = packet_in.altitude; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_set_global_origin_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_set_global_origin_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_set_global_origin_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_set_global_origin_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_set_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_set_global_origin_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_gps_set_global_origin_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_set_global_origin_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_set_global_origin_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_gps_local_origin_set(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gps_local_origin_set_t packet_in = { + 963497464, + 963497672, + 963497880, + }; + mavlink_gps_local_origin_set_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.latitude = packet_in.latitude; + packet1.longitude = packet_in.longitude; + packet1.altitude = packet_in.altitude; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_local_origin_set_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gps_local_origin_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_local_origin_set_pack(system_id, component_id, &msg , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_local_origin_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_local_origin_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_local_origin_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_gps_local_origin_set_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gps_local_origin_set_send(MAVLINK_COMM_1 , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_local_origin_set_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_local_position_setpoint_set(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_local_position_setpoint_set_t packet_in = { + 5, + 72, + 31.0, + 59.0, + 87.0, + 115.0, + }; + mavlink_local_position_setpoint_set_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.yaw = packet_in.yaw; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_set_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_local_position_setpoint_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_local_position_setpoint_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_local_position_setpoint_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_local_position_setpoint_set_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_set_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_local_position_setpoint_set_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_local_position_setpoint(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_local_position_setpoint_t packet_in = { + 17.0, + 45.0, + 73.0, + 101.0, + }; + mavlink_local_position_setpoint_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.yaw = packet_in.yaw; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_local_position_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_pack(system_id, component_id, &msg , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_local_position_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_local_position_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_local_position_setpoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_1 , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_local_position_setpoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_control_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_control_status_t packet_in = { + 5, + 72, + 139, + 206, + 17, + 84, + 151, + 218, + }; + mavlink_control_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.position_fix = packet_in.position_fix; + packet1.vision_fix = packet_in.vision_fix; + packet1.gps_fix = packet_in.gps_fix; + packet1.ahrs_health = packet_in.ahrs_health; + packet1.control_att = packet_in.control_att; + packet1.control_pos_xy = packet_in.control_pos_xy; + packet1.control_pos_z = packet_in.control_pos_z; + packet1.control_pos_yaw = packet_in.control_pos_yaw; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_control_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_control_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_control_status_pack(system_id, component_id, &msg , packet1.position_fix , packet1.vision_fix , packet1.gps_fix , packet1.ahrs_health , packet1.control_att , packet1.control_pos_xy , packet1.control_pos_z , packet1.control_pos_yaw ); + mavlink_msg_control_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_control_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.position_fix , packet1.vision_fix , packet1.gps_fix , packet1.ahrs_health , packet1.control_att , packet1.control_pos_xy , packet1.control_pos_z , packet1.control_pos_yaw ); + mavlink_msg_control_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_control_status_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_control_status_send(MAVLINK_COMM_1 , packet1.position_fix , packet1.vision_fix , packet1.gps_fix , packet1.ahrs_health , packet1.control_att , packet1.control_pos_xy , packet1.control_pos_z , packet1.control_pos_yaw ); + mavlink_msg_control_status_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_safety_set_allowed_area(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_safety_set_allowed_area_t packet_in = { + 5, + 72, + 139, + 38.0, + 66.0, + 94.0, + 122.0, + 150.0, + 178.0, + }; + mavlink_safety_set_allowed_area_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.frame = packet_in.frame; + packet1.p1x = packet_in.p1x; + packet1.p1y = packet_in.p1y; + packet1.p1z = packet_in.p1z; + packet1.p2x = packet_in.p2x; + packet1.p2y = packet_in.p2y; + packet1.p2z = packet_in.p2z; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_set_allowed_area_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_set_allowed_area_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_set_allowed_area_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_safety_set_allowed_area_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_set_allowed_area_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_set_allowed_area_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_safety_allowed_area(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_safety_allowed_area_t packet_in = { + 5, + 24.0, + 52.0, + 80.0, + 108.0, + 136.0, + 164.0, + }; + mavlink_safety_allowed_area_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.frame = packet_in.frame; + packet1.p1x = packet_in.p1x; + packet1.p1y = packet_in.p1y; + packet1.p1z = packet_in.p1z; + packet1.p2x = packet_in.p2x; + packet1.p2y = packet_in.p2y; + packet1.p2z = packet_in.p2z; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_allowed_area_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_safety_allowed_area_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_allowed_area_pack(system_id, component_id, &msg , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_allowed_area_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_allowed_area_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_allowed_area_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_safety_allowed_area_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_safety_allowed_area_send(MAVLINK_COMM_1 , packet1.frame , packet1.p1x , packet1.p1y , packet1.p1z , packet1.p2x , packet1.p2y , packet1.p2z ); + mavlink_msg_safety_allowed_area_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_set_roll_pitch_yaw_thrust(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_roll_pitch_yaw_thrust_t packet_in = { + 5, + 72, + 31.0, + 59.0, + 87.0, + 115.0, + }; + mavlink_set_roll_pitch_yaw_thrust_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.thrust = packet_in.thrust; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_thrust_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_roll_pitch_yaw_thrust_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust ); + mavlink_msg_set_roll_pitch_yaw_thrust_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust ); + mavlink_msg_set_roll_pitch_yaw_thrust_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_set_roll_pitch_yaw_thrust_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_thrust_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust ); + mavlink_msg_set_roll_pitch_yaw_thrust_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_set_roll_pitch_yaw_speed_thrust(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_roll_pitch_yaw_speed_thrust_t packet_in = { + 5, + 72, + 31.0, + 59.0, + 87.0, + 115.0, + }; + mavlink_set_roll_pitch_yaw_speed_thrust_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.roll_speed = packet_in.roll_speed; + packet1.pitch_speed = packet_in.pitch_speed; + packet1.yaw_speed = packet_in.yaw_speed; + packet1.thrust = packet_in.thrust; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust ); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust ); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust ); + mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_roll_pitch_yaw_thrust_setpoint(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_roll_pitch_yaw_thrust_setpoint_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + }; + mavlink_roll_pitch_yaw_thrust_setpoint_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_us = packet_in.time_us; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.thrust = packet_in.thrust; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, &msg , packet1.time_us , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust ); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_us , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust ); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_1 , packet1.time_us , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust ); + mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_roll_pitch_yaw_speed_thrust_setpoint(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + }; + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_us = packet_in.time_us; + packet1.roll_speed = packet_in.roll_speed; + packet1.pitch_speed = packet_in.pitch_speed; + packet1.yaw_speed = packet_in.yaw_speed; + packet1.thrust = packet_in.thrust; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, &msg , packet1.time_us , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust ); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_us , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust ); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(MAVLINK_COMM_1 , packet1.time_us , packet1.roll_speed , packet1.pitch_speed , packet1.yaw_speed , packet1.thrust ); + mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_nav_controller_output(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_nav_controller_output_t packet_in = { + 17.0, + 45.0, + 17651, + 17755, + 17859, + 115.0, + 143.0, + 171.0, + }; + mavlink_nav_controller_output_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.nav_roll = packet_in.nav_roll; + packet1.nav_pitch = packet_in.nav_pitch; + packet1.nav_bearing = packet_in.nav_bearing; + packet1.target_bearing = packet_in.target_bearing; + packet1.wp_dist = packet_in.wp_dist; + packet1.alt_error = packet_in.alt_error; + packet1.aspd_error = packet_in.aspd_error; + packet1.xtrack_error = packet_in.xtrack_error; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_nav_controller_output_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_nav_controller_output_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_nav_controller_output_pack(system_id, component_id, &msg , packet1.nav_roll , packet1.nav_pitch , packet1.nav_bearing , packet1.target_bearing , packet1.wp_dist , packet1.alt_error , packet1.aspd_error , packet1.xtrack_error ); + mavlink_msg_nav_controller_output_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_nav_controller_output_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.nav_roll , packet1.nav_pitch , packet1.nav_bearing , packet1.target_bearing , packet1.wp_dist , packet1.alt_error , packet1.aspd_error , packet1.xtrack_error ); + mavlink_msg_nav_controller_output_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_nav_controller_output_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_nav_controller_output_send(MAVLINK_COMM_1 , packet1.nav_roll , packet1.nav_pitch , packet1.nav_bearing , packet1.target_bearing , packet1.wp_dist , packet1.alt_error , packet1.aspd_error , packet1.xtrack_error ); + mavlink_msg_nav_controller_output_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_position_target(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_position_target_t packet_in = { + 17.0, + 45.0, + 73.0, + 101.0, + }; + mavlink_position_target_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.yaw = packet_in.yaw; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_position_target_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_position_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_position_target_pack(system_id, component_id, &msg , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_position_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_position_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_position_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_position_target_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_position_target_send(MAVLINK_COMM_1 , packet1.x , packet1.y , packet1.z , packet1.yaw ); + mavlink_msg_position_target_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_state_correction(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_state_correction_t packet_in = { + 17.0, + 45.0, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 241.0, + }; + mavlink_state_correction_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.xErr = packet_in.xErr; + packet1.yErr = packet_in.yErr; + packet1.zErr = packet_in.zErr; + packet1.rollErr = packet_in.rollErr; + packet1.pitchErr = packet_in.pitchErr; + packet1.yawErr = packet_in.yawErr; + packet1.vxErr = packet_in.vxErr; + packet1.vyErr = packet_in.vyErr; + packet1.vzErr = packet_in.vzErr; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_state_correction_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_state_correction_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_state_correction_pack(system_id, component_id, &msg , packet1.xErr , packet1.yErr , packet1.zErr , packet1.rollErr , packet1.pitchErr , packet1.yawErr , packet1.vxErr , packet1.vyErr , packet1.vzErr ); + mavlink_msg_state_correction_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_state_correction_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.xErr , packet1.yErr , packet1.zErr , packet1.rollErr , packet1.pitchErr , packet1.yawErr , packet1.vxErr , packet1.vyErr , packet1.vzErr ); + mavlink_msg_state_correction_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_state_correction_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_state_correction_send(MAVLINK_COMM_1 , packet1.xErr , packet1.yErr , packet1.zErr , packet1.rollErr , packet1.pitchErr , packet1.yawErr , packet1.vxErr , packet1.vyErr , packet1.vzErr ); + mavlink_msg_state_correction_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_set_altitude(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_set_altitude_t packet_in = { + 5, + 963497516, + }; + mavlink_set_altitude_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target = packet_in.target; + packet1.mode = packet_in.mode; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_altitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_set_altitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_altitude_pack(system_id, component_id, &msg , packet1.target , packet1.mode ); + mavlink_msg_set_altitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_altitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.mode ); + mavlink_msg_set_altitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_set_altitude_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_set_altitude_send(MAVLINK_COMM_1 , packet1.target , packet1.mode ); + mavlink_msg_set_altitude_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_request_data_stream(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_request_data_stream_t packet_in = { + 5, + 72, + 139, + 17391, + 84, + }; + mavlink_request_data_stream_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.req_stream_id = packet_in.req_stream_id; + packet1.req_message_rate = packet_in.req_message_rate; + packet1.start_stop = packet_in.start_stop; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_data_stream_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_request_data_stream_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_data_stream_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.req_stream_id , packet1.req_message_rate , packet1.start_stop ); + mavlink_msg_request_data_stream_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_data_stream_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.req_stream_id , packet1.req_message_rate , packet1.start_stop ); + mavlink_msg_request_data_stream_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_request_data_stream_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_request_data_stream_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.req_stream_id , packet1.req_message_rate , packet1.start_stop ); + mavlink_msg_request_data_stream_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_hil_state_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + 185.0, + 213.0, + 963499128, + 963499336, + 963499544, + 19523, + 19627, + 19731, + 19835, + 19939, + 20043, + }; + mavlink_hil_state_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.rollspeed = packet_in.rollspeed; + packet1.pitchspeed = packet_in.pitchspeed; + packet1.yawspeed = packet_in.yawspeed; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.xacc = packet_in.xacc; + packet1.yacc = packet_in.yacc; + packet1.zacc = packet_in.zacc; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_state_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_hil_state_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_state_pack(system_id, component_id, &msg , packet1.usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc ); + mavlink_msg_hil_state_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc ); + mavlink_msg_hil_state_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_hil_state_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_state_send(MAVLINK_COMM_1 , packet1.usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc ); + mavlink_msg_hil_state_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_hil_controls(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_hil_controls_t packet_in = { + 93372036854775807ULL, + 73.0, + 101.0, + 129.0, + 157.0, + 77, + 144, + }; + mavlink_hil_controls_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_us = packet_in.time_us; + packet1.roll_ailerons = packet_in.roll_ailerons; + packet1.pitch_elevator = packet_in.pitch_elevator; + packet1.yaw_rudder = packet_in.yaw_rudder; + packet1.throttle = packet_in.throttle; + packet1.mode = packet_in.mode; + packet1.nav_mode = packet_in.nav_mode; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_controls_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_hil_controls_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_controls_pack(system_id, component_id, &msg , packet1.time_us , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.mode , packet1.nav_mode ); + mavlink_msg_hil_controls_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_controls_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_us , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.mode , packet1.nav_mode ); + mavlink_msg_hil_controls_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_hil_controls_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_hil_controls_send(MAVLINK_COMM_1 , packet1.time_us , packet1.roll_ailerons , packet1.pitch_elevator , packet1.yaw_rudder , packet1.throttle , packet1.mode , packet1.nav_mode ); + mavlink_msg_hil_controls_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_manual_control(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_manual_control_t packet_in = { + 5, + 24.0, + 52.0, + 80.0, + 108.0, + 120, + 187, + 254, + 65, + }; + mavlink_manual_control_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target = packet_in.target; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.thrust = packet_in.thrust; + packet1.roll_manual = packet_in.roll_manual; + packet1.pitch_manual = packet_in.pitch_manual; + packet1.yaw_manual = packet_in.yaw_manual; + packet1.thrust_manual = packet_in.thrust_manual; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_manual_control_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_manual_control_pack(system_id, component_id, &msg , packet1.target , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.roll_manual , packet1.pitch_manual , packet1.yaw_manual , packet1.thrust_manual ); + mavlink_msg_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_manual_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.roll_manual , packet1.pitch_manual , packet1.yaw_manual , packet1.thrust_manual ); + mavlink_msg_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_manual_control_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_manual_control_send(MAVLINK_COMM_1 , packet1.target , packet1.roll , packet1.pitch , packet1.yaw , packet1.thrust , packet1.roll_manual , packet1.pitch_manual , packet1.yaw_manual , packet1.thrust_manual ); + mavlink_msg_manual_control_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_rc_channels_override_t packet_in = { + 5, + 72, + 17339, + 17443, + 17547, + 17651, + 17755, + 17859, + 17963, + 18067, + }; + mavlink_rc_channels_override_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.chan1_raw = packet_in.chan1_raw; + packet1.chan2_raw = packet_in.chan2_raw; + packet1.chan3_raw = packet_in.chan3_raw; + packet1.chan4_raw = packet_in.chan4_raw; + packet1.chan5_raw = packet_in.chan5_raw; + packet1.chan6_raw = packet_in.chan6_raw; + packet1.chan7_raw = packet_in.chan7_raw; + packet1.chan8_raw = packet_in.chan8_raw; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_override_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_rc_channels_override_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_override_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_rc_channels_override_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_rc_channels_override_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_global_position_int(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_global_position_int_t packet_in = { + 963497464, + 963497672, + 963497880, + 17859, + 17963, + 18067, + }; + mavlink_global_position_int_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_int_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_global_position_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_int_pack(system_id, component_id, &msg , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_global_position_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_global_position_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_global_position_int_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_global_position_int_send(MAVLINK_COMM_1 , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz ); + mavlink_msg_global_position_int_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_vfr_hud(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_vfr_hud_t packet_in = { + 17.0, + 45.0, + 17651, + 17755, + 101.0, + 129.0, + }; + mavlink_vfr_hud_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.airspeed = packet_in.airspeed; + packet1.groundspeed = packet_in.groundspeed; + packet1.heading = packet_in.heading; + packet1.throttle = packet_in.throttle; + packet1.alt = packet_in.alt; + packet1.climb = packet_in.climb; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_vfr_hud_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_vfr_hud_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_vfr_hud_pack(system_id, component_id, &msg , packet1.airspeed , packet1.groundspeed , packet1.heading , packet1.throttle , packet1.alt , packet1.climb ); + mavlink_msg_vfr_hud_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_vfr_hud_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.airspeed , packet1.groundspeed , packet1.heading , packet1.throttle , packet1.alt , packet1.climb ); + mavlink_msg_vfr_hud_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_vfr_hud_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_1 , packet1.airspeed , packet1.groundspeed , packet1.heading , packet1.throttle , packet1.alt , packet1.climb ); + mavlink_msg_vfr_hud_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_command(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_command_t packet_in = { + 5, + 72, + 139, + 206, + 45.0, + 73.0, + 101.0, + 129.0, + }; + mavlink_command_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.command = packet_in.command; + packet1.confirmation = packet_in.confirmation; + packet1.param1 = packet_in.param1; + packet1.param2 = packet_in.param2; + packet1.param3 = packet_in.param3; + packet1.param4 = packet_in.param4; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_command_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.command , packet1.confirmation , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 ); + mavlink_msg_command_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.command , packet1.confirmation , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 ); + mavlink_msg_command_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_command_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.command , packet1.confirmation , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 ); + mavlink_msg_command_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_command_ack_t packet_in = { + 17.0, + 45.0, + }; + mavlink_command_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.command = packet_in.command; + packet1.result = packet_in.result; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_command_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_ack_pack(system_id, component_id, &msg , packet1.command , packet1.result ); + mavlink_msg_command_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.result ); + mavlink_msg_command_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_command_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_command_ack_send(MAVLINK_COMM_1 , packet1.command , packet1.result ); + mavlink_msg_command_ack_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_optical_flow_t packet_in = { + 93372036854775807ULL, + 29, + 17703, + 17807, + 108, + 115.0, + }; + mavlink_optical_flow_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time = packet_in.time; + packet1.sensor_id = packet_in.sensor_id; + packet1.flow_x = packet_in.flow_x; + packet1.flow_y = packet_in.flow_y; + packet1.quality = packet_in.quality; + packet1.ground_distance = packet_in.ground_distance; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_optical_flow_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_optical_flow_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_optical_flow_pack(system_id, component_id, &msg , packet1.time , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_optical_flow_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_object_detection_event(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_object_detection_event_t packet_in = { + 963497464, + 17443, + 151, + "HIJKLMNOPQRSTUVWXYZ", + 22, + 213.0, + 241.0, + }; + mavlink_object_detection_event_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time = packet_in.time; + packet1.object_id = packet_in.object_id; + packet1.type = packet_in.type; + packet1.quality = packet_in.quality; + packet1.bearing = packet_in.bearing; + packet1.distance = packet_in.distance; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*20); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_object_detection_event_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_object_detection_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_object_detection_event_pack(system_id, component_id, &msg , packet1.time , packet1.object_id , packet1.type , packet1.name , packet1.quality , packet1.bearing , packet1.distance ); + mavlink_msg_object_detection_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_object_detection_event_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time , packet1.object_id , packet1.type , packet1.name , packet1.quality , packet1.bearing , packet1.distance ); + mavlink_msg_object_detection_event_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_object_detection_event_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_object_detection_event_send(MAVLINK_COMM_1 , packet1.time , packet1.object_id , packet1.type , packet1.name , packet1.quality , packet1.bearing , packet1.distance ); + mavlink_msg_object_detection_event_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_debug_vect(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_debug_vect_t packet_in = { + "ABCDEFGHI", + 93372036854776437ULL, + 143.0, + 171.0, + 199.0, + }; + mavlink_debug_vect_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_vect_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_debug_vect_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_vect_pack(system_id, component_id, &msg , packet1.name , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_debug_vect_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_vect_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.name , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_debug_vect_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_debug_vect_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_vect_send(MAVLINK_COMM_1 , packet1.name , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_debug_vect_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_named_value_float(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_named_value_float_t packet_in = { + "ABCDEFGHI", + 87.0, + }; + mavlink_named_value_float_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.value = packet_in.value; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_float_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_named_value_float_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_float_pack(system_id, component_id, &msg , packet1.name , packet1.value ); + mavlink_msg_named_value_float_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_float_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.name , packet1.value ); + mavlink_msg_named_value_float_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_named_value_float_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_float_send(MAVLINK_COMM_1 , packet1.name , packet1.value ); + mavlink_msg_named_value_float_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_named_value_int(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_named_value_int_t packet_in = { + "ABCDEFGHI", + 963497984, + }; + mavlink_named_value_int_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.value = packet_in.value; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_int_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_named_value_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_int_pack(system_id, component_id, &msg , packet1.name , packet1.value ); + mavlink_msg_named_value_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.name , packet1.value ); + mavlink_msg_named_value_int_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_named_value_int_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_named_value_int_send(MAVLINK_COMM_1 , packet1.name , packet1.value ); + mavlink_msg_named_value_int_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_statustext(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_statustext_t packet_in = { + 5, + { 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121 }, + }; + mavlink_statustext_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.severity = packet_in.severity; + + mav_array_memcpy(packet1.text, packet_in.text, sizeof(int8_t)*50); + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_statustext_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_statustext_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_statustext_pack(system_id, component_id, &msg , packet1.severity , packet1.text ); + mavlink_msg_statustext_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_statustext_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.severity , packet1.text ); + mavlink_msg_statustext_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_statustext_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_statustext_send(MAVLINK_COMM_1 , packet1.severity , packet1.text ); + mavlink_msg_statustext_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_debug(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_debug_t packet_in = { + 5, + 24.0, + }; + mavlink_debug_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.ind = packet_in.ind; + packet1.value = packet_in.value; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_debug_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_pack(system_id, component_id, &msg , packet1.ind , packet1.value ); + mavlink_msg_debug_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.ind , packet1.value ); + mavlink_msg_debug_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) { + comm_send_ch(MAVLINK_COMM_0, buffer[i]); + } + mavlink_msg_debug_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_send(MAVLINK_COMM_1 , packet1.ind , packet1.value ); + mavlink_msg_debug_decode(last_msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); +} + +static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_test_heartbeat(system_id, component_id, last_msg); + mavlink_test_boot(system_id, component_id, last_msg); + mavlink_test_system_time(system_id, component_id, last_msg); + mavlink_test_ping(system_id, component_id, last_msg); + mavlink_test_system_time_utc(system_id, component_id, last_msg); + mavlink_test_change_operator_control(system_id, component_id, last_msg); + mavlink_test_change_operator_control_ack(system_id, component_id, last_msg); + mavlink_test_auth_key(system_id, component_id, last_msg); + mavlink_test_action_ack(system_id, component_id, last_msg); + mavlink_test_action(system_id, component_id, last_msg); + mavlink_test_set_mode(system_id, component_id, last_msg); + mavlink_test_set_nav_mode(system_id, component_id, last_msg); + mavlink_test_param_request_read(system_id, component_id, last_msg); + mavlink_test_param_request_list(system_id, component_id, last_msg); + mavlink_test_param_value(system_id, component_id, last_msg); + mavlink_test_param_set(system_id, component_id, last_msg); + mavlink_test_gps_raw_int(system_id, component_id, last_msg); + mavlink_test_scaled_imu(system_id, component_id, last_msg); + mavlink_test_gps_status(system_id, component_id, last_msg); + mavlink_test_raw_imu(system_id, component_id, last_msg); + mavlink_test_raw_pressure(system_id, component_id, last_msg); + mavlink_test_scaled_pressure(system_id, component_id, last_msg); + mavlink_test_attitude(system_id, component_id, last_msg); + mavlink_test_local_position(system_id, component_id, last_msg); + mavlink_test_global_position(system_id, component_id, last_msg); + mavlink_test_gps_raw(system_id, component_id, last_msg); + mavlink_test_sys_status(system_id, component_id, last_msg); + mavlink_test_rc_channels_raw(system_id, component_id, last_msg); + mavlink_test_rc_channels_scaled(system_id, component_id, last_msg); + mavlink_test_servo_output_raw(system_id, component_id, last_msg); + mavlink_test_waypoint(system_id, component_id, last_msg); + mavlink_test_waypoint_request(system_id, component_id, last_msg); + mavlink_test_waypoint_set_current(system_id, component_id, last_msg); + mavlink_test_waypoint_current(system_id, component_id, last_msg); + mavlink_test_waypoint_request_list(system_id, component_id, last_msg); + mavlink_test_waypoint_count(system_id, component_id, last_msg); + mavlink_test_waypoint_clear_all(system_id, component_id, last_msg); + mavlink_test_waypoint_reached(system_id, component_id, last_msg); + mavlink_test_waypoint_ack(system_id, component_id, last_msg); + mavlink_test_gps_set_global_origin(system_id, component_id, last_msg); + mavlink_test_gps_local_origin_set(system_id, component_id, last_msg); + mavlink_test_local_position_setpoint_set(system_id, component_id, last_msg); + mavlink_test_local_position_setpoint(system_id, component_id, last_msg); + mavlink_test_control_status(system_id, component_id, last_msg); + mavlink_test_safety_set_allowed_area(system_id, component_id, last_msg); + mavlink_test_safety_allowed_area(system_id, component_id, last_msg); + mavlink_test_set_roll_pitch_yaw_thrust(system_id, component_id, last_msg); + mavlink_test_set_roll_pitch_yaw_speed_thrust(system_id, component_id, last_msg); + mavlink_test_roll_pitch_yaw_thrust_setpoint(system_id, component_id, last_msg); + mavlink_test_roll_pitch_yaw_speed_thrust_setpoint(system_id, component_id, last_msg); + mavlink_test_nav_controller_output(system_id, component_id, last_msg); + mavlink_test_position_target(system_id, component_id, last_msg); + mavlink_test_state_correction(system_id, component_id, last_msg); + mavlink_test_set_altitude(system_id, component_id, last_msg); + mavlink_test_request_data_stream(system_id, component_id, last_msg); + mavlink_test_hil_state(system_id, component_id, last_msg); + mavlink_test_hil_controls(system_id, component_id, last_msg); + mavlink_test_manual_control(system_id, component_id, last_msg); + mavlink_test_rc_channels_override(system_id, component_id, last_msg); + mavlink_test_global_position_int(system_id, component_id, last_msg); + mavlink_test_vfr_hud(system_id, component_id, last_msg); + mavlink_test_command(system_id, component_id, last_msg); + mavlink_test_command_ack(system_id, component_id, last_msg); + mavlink_test_optical_flow(system_id, component_id, last_msg); + mavlink_test_object_detection_event(system_id, component_id, last_msg); + mavlink_test_debug_vect(system_id, component_id, last_msg); + mavlink_test_named_value_float(system_id, component_id, last_msg); + mavlink_test_named_value_int(system_id, component_id, last_msg); + mavlink_test_statustext(system_id, component_id, last_msg); + mavlink_test_debug(system_id, component_id, last_msg); +} + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // COMMON_TESTSUITE_H diff --git a/mavlink/include/mavlink/v0.9/common/version.h b/mavlink/include/mavlink/v0.9/common/version.h new file mode 100644 index 000000000..5ca3dc8ca --- /dev/null +++ b/mavlink/include/mavlink/v0.9/common/version.h @@ -0,0 +1,12 @@ +/** @file + * @brief MAVLink comm protocol built from common.xml + * @see http://pixhawk.ethz.ch/software/mavlink + */ +#ifndef MAVLINK_VERSION_H +#define MAVLINK_VERSION_H + +#define MAVLINK_BUILD_DATE "Fri Apr 20 12:22:54 2012" +#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9" +#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 101 + +#endif // MAVLINK_VERSION_H |