diff --git a/libraries/GCS_MAVLink/GCS_MAVLink.h b/libraries/GCS_MAVLink/GCS_MAVLink.h index 3042553f98..d94c8853a6 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink.h +++ b/libraries/GCS_MAVLink/GCS_MAVLink.h @@ -7,6 +7,8 @@ #define GCS_MAVLink_h #include + +#define MAVLINK_COMM_NUM_BUFFERS 2 #include "include/mavlink_types.h" /// MAVLink stream used for HIL interaction diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 64827032d4..fa3beb36fc 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from ardupilotmega.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 13 2011, 08:44 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -10,39 +9,46 @@ 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, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, {}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {}, 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, {}, {}, {}, 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, {}, {}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, 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_ARDUPILOTMEGA - #include "../common/common.h" + // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 0 +#define MAVLINK_VERSION 2 #endif // ENUM DEFINITIONS -// MESSAGE DEFINITIONS +// MESSAGE DEFINITIONS #include "./mavlink_msg_sensor_offsets.h" #include "./mavlink_msg_set_mag_offsets.h" - -// MESSAGE LENGTHS - -#undef 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, 20, 20, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 } - #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // ARDUPILOTMEGA_H diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h index a54a6df31b..72e0248d98 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h @@ -1,11 +1,27 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol built from ardupilotmega.xml * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, August 13 2011, 08:44 UTC */ #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 "ardupilotmega.h" -#endif +#endif // MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_sensor_offsets.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_sensor_offsets.h index a3191c7870..d88471380d 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -2,23 +2,44 @@ #define MAVLINK_MSG_ID_SENSOR_OFFSETS 150 -typedef struct __mavlink_sensor_offsets_t +typedef struct __mavlink_sensor_offsets_t { - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset - float mag_declination; ///< magnetic declination (radians) - int32_t raw_press; ///< raw pressure from barometer - int32_t raw_temp; ///< raw temperature from barometer - float gyro_cal_x; ///< gyro X calibration - float gyro_cal_y; ///< gyro Y calibration - float gyro_cal_z; ///< gyro Z calibration - float accel_cal_x; ///< accel X calibration - float accel_cal_y; ///< accel Y calibration - float accel_cal_z; ///< accel Z calibration - + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset + float mag_declination; ///< magnetic declination (radians) + int32_t raw_press; ///< raw pressure from barometer + int32_t raw_temp; ///< raw temperature from barometer + float gyro_cal_x; ///< gyro X calibration + float gyro_cal_y; ///< gyro Y calibration + float gyro_cal_z; ///< gyro Z calibration + float accel_cal_x; ///< accel X calibration + float accel_cal_y; ///< accel Y calibration + float accel_cal_z; ///< accel Z calibration } mavlink_sensor_offsets_t; +#define MAVLINK_MSG_ID_SENSOR_OFFSETS_LEN 42 +#define MAVLINK_MSG_ID_150_LEN 42 + + + +#define MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS { \ + "SENSOR_OFFSETS", \ + 12, \ + { { "mag_ofs_x", MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_sensor_offsets_t, mag_ofs_x) }, \ + { "mag_ofs_y", MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_sensor_offsets_t, mag_ofs_y) }, \ + { "mag_ofs_z", MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_sensor_offsets_t, mag_ofs_z) }, \ + { "mag_declination", MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_sensor_offsets_t, mag_declination) }, \ + { "raw_press", MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_sensor_offsets_t, raw_press) }, \ + { "raw_temp", MAVLINK_TYPE_INT32_T, 0, 14, offsetof(mavlink_sensor_offsets_t, raw_temp) }, \ + { "gyro_cal_x", MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_sensor_offsets_t, gyro_cal_x) }, \ + { "gyro_cal_y", MAVLINK_TYPE_FLOAT, 0, 22, offsetof(mavlink_sensor_offsets_t, gyro_cal_y) }, \ + { "gyro_cal_z", MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_sensor_offsets_t, gyro_cal_z) }, \ + { "accel_cal_x", MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_sensor_offsets_t, accel_cal_x) }, \ + { "accel_cal_y", MAVLINK_TYPE_FLOAT, 0, 34, offsetof(mavlink_sensor_offsets_t, accel_cal_y) }, \ + { "accel_cal_z", MAVLINK_TYPE_FLOAT, 0, 38, offsetof(mavlink_sensor_offsets_t, accel_cal_z) }, \ + } \ +} /** @@ -41,29 +62,49 @@ typedef struct __mavlink_sensor_offsets_t * @param accel_cal_z accel Z calibration * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_float(buf, 6, mag_declination); + _mav_put_int32_t(buf, 10, raw_press); + _mav_put_int32_t(buf, 14, raw_temp); + _mav_put_float(buf, 18, gyro_cal_x); + _mav_put_float(buf, 22, gyro_cal_y); + _mav_put_float(buf, 26, gyro_cal_z); + _mav_put_float(buf, 30, accel_cal_x); + _mav_put_float(buf, 34, accel_cal_y); + _mav_put_float(buf, 38, accel_cal_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_sensor_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - - i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset - i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset - i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset - i += put_float_by_index(mag_declination, i, msg->payload); // magnetic declination (radians) - i += put_int32_t_by_index(raw_press, i, msg->payload); // raw pressure from barometer - i += put_int32_t_by_index(raw_temp, i, msg->payload); // raw temperature from barometer - i += put_float_by_index(gyro_cal_x, i, msg->payload); // gyro X calibration - i += put_float_by_index(gyro_cal_y, i, msg->payload); // gyro Y calibration - i += put_float_by_index(gyro_cal_z, i, msg->payload); // gyro Z calibration - i += put_float_by_index(accel_cal_x, i, msg->payload); // accel X calibration - i += put_float_by_index(accel_cal_y, i, msg->payload); // accel Y calibration - i += put_float_by_index(accel_cal_z, i, msg->payload); // accel Z calibration - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 42); } /** - * @brief Pack a sensor_offsets message + * @brief Pack a sensor_offsets 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 @@ -82,25 +123,46 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_ * @param accel_cal_z accel Z calibration * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) +static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_float(buf, 6, mag_declination); + _mav_put_int32_t(buf, 10, raw_press); + _mav_put_int32_t(buf, 14, raw_temp); + _mav_put_float(buf, 18, gyro_cal_x); + _mav_put_float(buf, 22, gyro_cal_y); + _mav_put_float(buf, 26, gyro_cal_z); + _mav_put_float(buf, 30, accel_cal_x); + _mav_put_float(buf, 34, accel_cal_y); + _mav_put_float(buf, 38, accel_cal_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_sensor_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - - i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset - i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset - i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset - i += put_float_by_index(mag_declination, i, msg->payload); // magnetic declination (radians) - i += put_int32_t_by_index(raw_press, i, msg->payload); // raw pressure from barometer - i += put_int32_t_by_index(raw_temp, i, msg->payload); // raw temperature from barometer - i += put_float_by_index(gyro_cal_x, i, msg->payload); // gyro X calibration - i += put_float_by_index(gyro_cal_y, i, msg->payload); // gyro Y calibration - i += put_float_by_index(gyro_cal_z, i, msg->payload); // gyro Z calibration - i += put_float_by_index(accel_cal_x, i, msg->payload); // accel X calibration - i += put_float_by_index(accel_cal_y, i, msg->payload); // accel Y calibration - i += put_float_by_index(accel_cal_z, i, msg->payload); // accel Z calibration - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42); } /** @@ -137,14 +199,46 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) { - mavlink_message_t msg; - mavlink_msg_sensor_offsets_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z); - mavlink_send_uart(chan, &msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_float(buf, 6, mag_declination); + _mav_put_int32_t(buf, 10, raw_press); + _mav_put_int32_t(buf, 14, raw_temp); + _mav_put_float(buf, 18, gyro_cal_x); + _mav_put_float(buf, 22, gyro_cal_y); + _mav_put_float(buf, 26, gyro_cal_z); + _mav_put_float(buf, 30, accel_cal_x); + _mav_put_float(buf, 34, accel_cal_y); + _mav_put_float(buf, 38, accel_cal_z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, 42); +#else + mavlink_sensor_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, 42); +#endif } #endif + // MESSAGE SENSOR_OFFSETS UNPACKING + /** * @brief Get field mag_ofs_x from sensor_offsets message * @@ -152,10 +246,7 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16 */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 0); } /** @@ -165,10 +256,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_mes */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 2); } /** @@ -178,10 +266,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_mes */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 4); } /** @@ -191,12 +276,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 6); } /** @@ -206,12 +286,7 @@ static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink */ static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 10); } /** @@ -221,12 +296,7 @@ static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_mes */ static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 14); } /** @@ -236,12 +306,7 @@ static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 18); } /** @@ -251,12 +316,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 22); } /** @@ -266,12 +326,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 26); } /** @@ -281,12 +336,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 30); } /** @@ -296,12 +346,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 34); } /** @@ -311,12 +356,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 38); } /** @@ -327,6 +367,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_mes */ static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets) { +#if MAVLINK_NEED_BYTE_SWAP sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg); sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); @@ -339,4 +380,7 @@ static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* ms sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg); sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg); sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg); +#else + memcpy(sensor_offsets, _MAV_PAYLOAD(msg), 42); +#endif } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h index 10dd1a58fd..96f09664e6 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -2,16 +2,30 @@ #define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151 -typedef struct __mavlink_set_mag_offsets_t +typedef struct __mavlink_set_mag_offsets_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - int16_t mag_ofs_x; ///< magnetometer X offset - int16_t mag_ofs_y; ///< magnetometer Y offset - int16_t mag_ofs_z; ///< magnetometer Z offset - + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + int16_t mag_ofs_x; ///< magnetometer X offset + int16_t mag_ofs_y; ///< magnetometer Y offset + int16_t mag_ofs_z; ///< magnetometer Z offset } mavlink_set_mag_offsets_t; +#define MAVLINK_MSG_ID_SET_MAG_OFFSETS_LEN 8 +#define MAVLINK_MSG_ID_151_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS { \ + "SET_MAG_OFFSETS", \ + 5, \ + { { "target_system", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_mag_offsets_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_mag_offsets_t, target_component) }, \ + { "mag_ofs_x", MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_set_mag_offsets_t, mag_ofs_x) }, \ + { "mag_ofs_y", MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_set_mag_offsets_t, mag_ofs_y) }, \ + { "mag_ofs_z", MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_set_mag_offsets_t, mag_ofs_z) }, \ + } \ +} /** @@ -27,22 +41,35 @@ typedef struct __mavlink_set_mag_offsets_t * @param mag_ofs_z magnetometer Z offset * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int16_t(buf, 2, mag_ofs_x); + _mav_put_int16_t(buf, 4, mag_ofs_y); + _mav_put_int16_t(buf, 6, mag_ofs_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_set_mag_offsets_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset - i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset - i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 8); } /** - * @brief Pack a set_mag_offsets message + * @brief Pack a set_mag_offsets 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 @@ -54,18 +81,32 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8 * @param mag_ofs_z magnetometer Z offset * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_set_mag_offsets_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, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) +static inline uint16_t mavlink_msg_set_mag_offsets_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,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int16_t(buf, 2, mag_ofs_x); + _mav_put_int16_t(buf, 4, mag_ofs_y); + _mav_put_int16_t(buf, 6, mag_ofs_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_set_mag_offsets_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset - i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset - i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); } /** @@ -95,14 +136,32 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) { - mavlink_message_t msg; - mavlink_msg_set_mag_offsets_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z); - mavlink_send_uart(chan, &msg); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_int16_t(buf, 2, mag_ofs_x); + _mav_put_int16_t(buf, 4, mag_ofs_y); + _mav_put_int16_t(buf, 6, mag_ofs_z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, 8); +#else + mavlink_set_mag_offsets_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, 8); +#endif } #endif + // MESSAGE SET_MAG_OFFSETS UNPACKING + /** * @brief Get field target_system from set_mag_offsets message * @@ -110,7 +169,7 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +179,7 @@ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlin */ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -130,10 +189,7 @@ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mav */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 2); } /** @@ -143,10 +199,7 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_me */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 4); } /** @@ -156,10 +209,7 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_me */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 6); } /** @@ -170,9 +220,13 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_me */ static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets) { +#if MAVLINK_NEED_BYTE_SWAP set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg); set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg); set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg); +#else + memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), 8); +#endif } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h new file mode 100644 index 0000000000..3d0469e597 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -0,0 +1,152 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from ardupilotmega.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#ifndef ARDUPILOTMEGA_TESTSUITE_H +#define ARDUPILOTMEGA_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_ardupilotmega(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); + mavlink_test_ardupilotmega(system_id, component_id, last_msg); +} +#endif + +#include "../common/testsuite.h" + + +static void mavlink_test_sensor_offsets(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_sensor_offsets_t packet_in = { + 17235, + 17339, + 17443, + 59.0, + 963497984, + 963498192, + 143.0, + 171.0, + 199.0, + 227.0, + 255.0, + 283.0, + }; + mavlink_sensor_offsets_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mag_ofs_x = packet_in.mag_ofs_x; + packet1.mag_ofs_y = packet_in.mag_ofs_y; + packet1.mag_ofs_z = packet_in.mag_ofs_z; + packet1.mag_declination = packet_in.mag_declination; + packet1.raw_press = packet_in.raw_press; + packet1.raw_temp = packet_in.raw_temp; + packet1.gyro_cal_x = packet_in.gyro_cal_x; + packet1.gyro_cal_y = packet_in.gyro_cal_y; + packet1.gyro_cal_z = packet_in.gyro_cal_z; + packet1.accel_cal_x = packet_in.accel_cal_x; + packet1.accel_cal_y = packet_in.accel_cal_y; + packet1.accel_cal_z = packet_in.accel_cal_z; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_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>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); } @@ -54,35 +54,31 @@ static inline void crc_init(uint16_t* crcAccum) * @param length length of the byte array * @return the checksum over the buffer bytes **/ -static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +static inline uint16_t crc_calculate(uint8_t* pBuffer, uint16_t length) { - - // For a "message" of length bytes contained in the unsigned char array - // pointed to by pBuffer, calculate the CRC - // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed - uint16_t crcTmp; - //uint16_t tmp; - uint8_t* pTmp; - int i; - - pTmp=pBuffer; - - - /* init crcTmp */ crc_init(&crcTmp); - - for (i = 0; i < length; i++){ - crc_accumulate(*pTmp++, &crcTmp); + while (length--) { + crc_accumulate(*pBuffer++, &crcTmp); } + return crcTmp; +} - /* This is currently not needed, as only the checksum over payload should be computed - tmp = crcTmp; - crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); - crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); - *checkConst = tmp; - */ - return(crcTmp); +/** + * @brief Accumulate the X.25 CRC by adding an array of bytes + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new bytes to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length) +{ + const uint8_t *p = (const uint8_t *)pBuffer; + while (length--) { + crc_accumulate(*p++, crcAccum); + } } diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index b4e0eb96ec..4a74147be4 100644 --- a/libraries/GCS_MAVLink/include/common/common.h +++ b/libraries/GCS_MAVLink/include/common/common.h @@ -1,7 +1,6 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol generated from common.xml * @see http://qgroundcontrol.org/mavlink/ - * Generated on Saturday, August 13 2011, 08:44 UTC */ #ifndef COMMON_H #define COMMON_H @@ -10,11 +9,26 @@ 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, {}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {}, 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, {}, {}, {}, 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, {}, {}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, 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 @@ -28,42 +42,57 @@ extern "C" { // 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. */ + +/** @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. */ 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 + 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, /* | */ }; -/** @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. */ +/** @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. + */ enum MAV_DATA_STREAM { MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ @@ -75,10 +104,13 @@ enum MAV_DATA_STREAM 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 + MAV_DATA_STREAM_ENUM_END=13, /* | */ }; -/** @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). */ +/** @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). + */ enum MAV_ROI { MAV_ROI_NONE=0, /* No region of interest. | */ @@ -86,12 +118,10 @@ enum MAV_ROI 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 + MAV_ROI_ENUM_END=5, /* | */ }; - // MESSAGE DEFINITIONS - #include "./mavlink_msg_heartbeat.h" #include "./mavlink_msg_boot.h" #include "./mavlink_msg_system_time.h" @@ -155,19 +185,15 @@ enum MAV_ROI #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" - -// MESSAGE LENGTHS - -#undef 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, 20, 20, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 } - #ifdef __cplusplus } -#endif -#endif +#endif // __cplusplus +#endif // COMMON_H diff --git a/libraries/GCS_MAVLink/include/common/mavlink.h b/libraries/GCS_MAVLink/include/common/mavlink.h index 7c861f020c..02ff5bd392 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink.h +++ b/libraries/GCS_MAVLink/include/common/mavlink.h @@ -1,11 +1,27 @@ /** @file - * @brief MAVLink comm protocol. + * @brief MAVLink comm protocol built from common.xml * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Saturday, August 13 2011, 08:44 UTC */ #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 +#endif // MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_action.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_action.h index f1de546754..43639cf546 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_action.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_action.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_ACTION 10 -typedef struct __mavlink_action_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_action_t, target) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_action_t, target_component) }, \ + { "action", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_action_t, action) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_action_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, action); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_action_t packet; + packet.target = target; + packet.target_component = target_component; + packet.action = action; + + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_ACTION; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action - i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 3); } /** - * @brief Pack a action message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t compon * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, action); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_action_t packet; + packet.target = target; + packet.target_component = target_component; + packet.action = action; + + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_ACTION; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system executing the action - i += put_uint8_t_by_index(target_component, i, msg->payload); // The component executing the action - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t comp static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) { - mavlink_message_t msg; - mavlink_msg_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, target_component, action); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,7 +147,7 @@ static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t targe */ static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -108,7 +157,7 @@ static inline uint8_t mavlink_msg_action_get_target(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -118,7 +167,7 @@ static inline uint8_t mavlink_msg_action_get_target_component(const mavlink_mess */ static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -129,7 +178,11 @@ static inline uint8_t mavlink_msg_action_get_action(const mavlink_message_t* msg */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h index 6e93449e20..21daa2942e 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_action_ack.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_ACTION_ACK 9 -typedef struct __mavlink_action_ack_t +typedef struct __mavlink_action_ack_t { - uint8_t action; ///< The action id - uint8_t result; ///< 0: Action DENIED, 1: Action executed - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_action_ack_t, action) }, \ + { "result", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_action_ack_t, result) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_action_ack_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, action); + _mav_put_uint8_t(buf, 1, result); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_action_ack_t packet; + packet.action = action; + packet.result = result; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - i += put_uint8_t_by_index(result, i, msg->payload); // 0: Action DENIED, 1: Action executed - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a action_ack message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t co * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, action); + _mav_put_uint8_t(buf, 1, result); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_action_ack_t packet; + packet.action = action; + packet.result = result; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_ACTION_ACK; - - i += put_uint8_t_by_index(action, i, msg->payload); // The action id - i += put_uint8_t_by_index(result, i, msg->payload); // 0: Action DENIED, 1: Action executed - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) { - mavlink_message_t msg; - mavlink_msg_action_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, action, result); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t a */ static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,7 +146,7 @@ static inline uint8_t mavlink_msg_action_ack_get_action(const mavlink_message_t* */ static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -113,6 +157,10 @@ static inline uint8_t mavlink_msg_action_ack_get_result(const mavlink_message_t* */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude.h index 6daba3c54b..0387be6ef4 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_attitude.h @@ -2,18 +2,34 @@ #define MAVLINK_MSG_ID_ATTITUDE 30 -typedef struct __mavlink_attitude_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_attitude_t, usec) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_attitude_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_attitude_t, yaw) }, \ + { "rollspeed", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_t, rollspeed) }, \ + { "pitchspeed", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_t, pitchspeed) }, \ + { "yawspeed", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_t, yawspeed) }, \ + } \ +} /** @@ -31,24 +47,39 @@ typedef struct __mavlink_attitude_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 32); } /** - * @brief Pack a attitude message + * @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 @@ -62,20 +93,36 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); } /** @@ -107,14 +154,36 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co 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) { - mavlink_message_t msg; - mavlink_msg_attitude_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); - mavlink_send_uart(chan, &msg); +#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 * @@ -122,16 +191,7 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t us */ static inline uint64_t mavlink_msg_attitude_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -141,12 +201,7 @@ static inline uint64_t mavlink_msg_attitude_get_usec(const mavlink_message_t* ms */ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -156,12 +211,7 @@ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -171,12 +221,7 @@ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -186,12 +231,7 @@ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -201,12 +241,7 @@ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 24); } /** @@ -216,12 +251,7 @@ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 28); } /** @@ -232,6 +262,7 @@ static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* m */ 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); @@ -239,4 +270,7 @@ static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mav 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/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h index 605ba7db61..33a05efda5 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_auth_key.h @@ -2,14 +2,23 @@ #define MAVLINK_MSG_ID_AUTH_KEY 7 -typedef struct __mavlink_auth_key_t +typedef struct __mavlink_auth_key_t { - char key[32]; ///< key - + 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", MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_auth_key_t, key) }, \ + } \ +} + /** * @brief Pack a auth_key message @@ -20,18 +29,27 @@ typedef struct __mavlink_auth_key_t * @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) +static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *key) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_auth_key_t packet; + + memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 32); } /** - * @brief Pack a auth_key message + * @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 @@ -39,14 +57,24 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp * @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) +static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *key) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_auth_key_t packet; + + memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); } /** @@ -70,26 +98,34 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) { - mavlink_message_t msg; - mavlink_msg_auth_key_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, key); - mavlink_send_uart(chan, &msg); +#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; + + 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* r_data) +static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) { - - memcpy(r_data, msg->payload, sizeof(char)*32); - return sizeof(char)*32; + return _MAV_RETURN_char_array(msg, key, 32, 0); } /** @@ -100,5 +136,9 @@ static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_boot.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_boot.h index 69d8ed64a7..68c3c0b8fa 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_boot.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_boot.h @@ -2,12 +2,22 @@ #define MAVLINK_MSG_ID_BOOT 1 -typedef struct __mavlink_boot_t +typedef struct __mavlink_boot_t { - uint32_t version; ///< The onboard software version - + 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", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_boot_t, version) }, \ + } \ +} /** @@ -19,18 +29,27 @@ typedef struct __mavlink_boot_t * @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) +static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t version) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint32_t(buf, 0, version); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_boot_t packet; + packet.version = version; + + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_BOOT; - - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 4); } /** - * @brief Pack a boot message + * @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 @@ -38,14 +57,24 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint32_t(buf, 0, version); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_boot_t packet; + packet.version = version; + + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_BOOT; - - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); } /** @@ -71,14 +100,24 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { - mavlink_message_t msg; - mavlink_msg_boot_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, version); - mavlink_send_uart(chan, &msg); +#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 * @@ -86,12 +125,7 @@ static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t versio */ static inline uint32_t mavlink_msg_boot_get_version(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -102,5 +136,9 @@ static inline uint32_t mavlink_msg_boot_get_version(const mavlink_message_t* msg */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control.h index 3eab478cbf..3837e9f5d5 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control.h @@ -2,17 +2,29 @@ #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 -typedef struct __mavlink_change_operator_control_t +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 "!?,.-" - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_t, target_system) }, \ + { "control_request", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_t, control_request) }, \ + { "version", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_t, version) }, \ + { "passkey", MAVLINK_TYPE_CHAR, 25, 3, offsetof(mavlink_change_operator_control_t, passkey) }, \ + } \ +} + /** * @brief Pack a change_operator_control message @@ -26,21 +38,31 @@ typedef struct __mavlink_change_operator_control_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System the GCS requests control for - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(version, i, msg->payload); // 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. - i += put_array_by_index((const int8_t*)passkey, sizeof(char)*25, i, msg->payload); // 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 mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 28); } /** - * @brief Pack a change_operator_control message + * @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 @@ -51,17 +73,28 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System the GCS requests control for - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(version, i, msg->payload); // 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. - i += put_array_by_index((const int8_t*)passkey, sizeof(char)*25, i, msg->payload); // 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 mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28); } /** @@ -88,16 +121,30 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system */ #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) +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) { - mavlink_message_t msg; - mavlink_msg_change_operator_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, control_request, version, passkey); - mavlink_send_uart(chan, &msg); +#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; + 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 * @@ -105,7 +152,7 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -115,7 +162,7 @@ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(cons */ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -125,7 +172,7 @@ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(co */ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -133,11 +180,9 @@ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavl * * @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* r_data) +static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char *passkey) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t), sizeof(char)*25); - return sizeof(char)*25; + return _MAV_RETURN_char_array(msg, passkey, 25, 3); } /** @@ -148,8 +193,12 @@ static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mav */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control_ack.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control_ack.h index 3529ae0591..27b3e12d00 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control_ack.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_change_operator_control_ack.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 -typedef struct __mavlink_change_operator_control_ack_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_change_operator_control_ack_t, gcs_system_id) }, \ + { "control_request", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_change_operator_control_ack_t, control_request) }, \ + { "ack", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_change_operator_control_ack_t, ack) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_change_operator_control_ack_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - - i += put_uint8_t_by_index(gcs_system_id, i, msg->payload); // ID of the GCS this message - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(ack, i, msg->payload); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 3); } /** - * @brief Pack a change_operator_control_ack message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - - i += put_uint8_t_by_index(gcs_system_id, i, msg->payload); // ID of the GCS this message - i += put_uint8_t_by_index(control_request, i, msg->payload); // 0: request control of this MAV, 1: Release control of this MAV - i += put_uint8_t_by_index(ack, i, msg->payload); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy 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) { - mavlink_message_t msg; - mavlink_msg_change_operator_control_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, gcs_system_id, control_request, ack); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,7 +147,7 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -108,7 +157,7 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id( */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -118,7 +167,7 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_reques */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -129,7 +178,11 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavl */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_command.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_command.h index dbc1235707..065893327f 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_command.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_command.h @@ -2,19 +2,36 @@ #define MAVLINK_MSG_ID_COMMAND 75 -typedef struct __mavlink_command_t +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. - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_command_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_command_t, target_component) }, \ + { "command", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_t, command) }, \ + { "confirmation", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_command_t, confirmation) }, \ + { "param1", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_t, param1) }, \ + { "param2", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_command_t, param2) }, \ + { "param3", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_command_t, param3) }, \ + { "param4", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_command_t, param4) }, \ + } \ +} /** @@ -33,25 +50,41 @@ typedef struct __mavlink_command_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components - i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum. - i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum. - i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum. - i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum. - i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum. - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 20); } /** - * @brief Pack a command message + * @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 @@ -66,21 +99,38 @@ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t compo * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components - i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum. - i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum. - i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum. - i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum. - i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum. - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20); } /** @@ -113,14 +163,38 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com 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) { - mavlink_message_t msg; - mavlink_msg_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, command, confirmation, param1, param2, param3, param4); - mavlink_send_uart(chan, &msg); +#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 * @@ -128,7 +202,7 @@ static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t targ */ static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -138,7 +212,7 @@ static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_messag */ static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -148,7 +222,7 @@ static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_mes */ static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -158,7 +232,7 @@ static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -168,12 +242,7 @@ static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message */ static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -183,12 +252,7 @@ static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -198,12 +262,7 @@ static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -213,12 +272,7 @@ static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg) */ static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -229,6 +283,7 @@ static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg) */ 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); @@ -237,4 +292,7 @@ static inline void mavlink_msg_command_decode(const mavlink_message_t* msg, mavl 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/libraries/GCS_MAVLink/include/common/mavlink_msg_command_ack.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_command_ack.h index 32b39da3a6..be37eeca73 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_command_ack.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_command_ack.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_COMMAND_ACK 76 -typedef struct __mavlink_command_ack_t +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 - + 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", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ + { "result", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_command_ack_t, result) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_command_ack_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - - i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 8); } /** - * @brief Pack a command_ack message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - - i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { - mavlink_message_t msg; - mavlink_msg_command_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, command, result); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,12 +136,7 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float co */ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 0); } /** @@ -107,12 +146,7 @@ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* */ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -123,6 +157,10 @@ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h index 746e889aaf..534abe60e0 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_control_status.h @@ -2,19 +2,36 @@ #define MAVLINK_MSG_ID_CONTROL_STATUS 52 -typedef struct __mavlink_control_status_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_control_status_t, position_fix) }, \ + { "vision_fix", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_control_status_t, vision_fix) }, \ + { "gps_fix", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_control_status_t, gps_fix) }, \ + { "ahrs_health", MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_control_status_t, ahrs_health) }, \ + { "control_att", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_control_status_t, control_att) }, \ + { "control_pos_xy", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_control_status_t, control_pos_xy) }, \ + { "control_pos_z", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_control_status_t, control_pos_z) }, \ + { "control_pos_yaw", MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_control_status_t, control_pos_yaw) }, \ + } \ +} /** @@ -33,25 +50,41 @@ typedef struct __mavlink_control_status_t * @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) +static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, position_fix); + _mav_put_uint8_t(buf, 1, vision_fix); + _mav_put_uint8_t(buf, 2, gps_fix); + _mav_put_uint8_t(buf, 3, ahrs_health); + _mav_put_uint8_t(buf, 4, control_att); + _mav_put_uint8_t(buf, 5, control_pos_xy); + _mav_put_uint8_t(buf, 6, control_pos_z); + _mav_put_uint8_t(buf, 7, control_pos_yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_control_status_t packet; + packet.position_fix = position_fix; + packet.vision_fix = vision_fix; + packet.gps_fix = gps_fix; + packet.ahrs_health = ahrs_health; + packet.control_att = control_att; + packet.control_pos_xy = control_pos_xy; + packet.control_pos_z = control_pos_z; + packet.control_pos_yaw = control_pos_yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 8); } /** - * @brief Pack a control_status message + * @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 @@ -66,21 +99,38 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ * @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) +static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t position_fix,uint8_t vision_fix,uint8_t gps_fix,uint8_t ahrs_health,uint8_t control_att,uint8_t control_pos_xy,uint8_t control_pos_z,uint8_t control_pos_yaw) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, position_fix); + _mav_put_uint8_t(buf, 1, vision_fix); + _mav_put_uint8_t(buf, 2, gps_fix); + _mav_put_uint8_t(buf, 3, ahrs_health); + _mav_put_uint8_t(buf, 4, control_att); + _mav_put_uint8_t(buf, 5, control_pos_xy); + _mav_put_uint8_t(buf, 6, control_pos_z); + _mav_put_uint8_t(buf, 7, control_pos_yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_control_status_t packet; + packet.position_fix = position_fix; + packet.vision_fix = vision_fix; + packet.gps_fix = gps_fix; + packet.ahrs_health = ahrs_health; + packet.control_att = control_att; + packet.control_pos_xy = control_pos_xy; + packet.control_pos_z = control_pos_z; + packet.control_pos_yaw = control_pos_yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); } /** @@ -113,14 +163,38 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { - mavlink_message_t msg; - mavlink_msg_control_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw); - mavlink_send_uart(chan, &msg); +#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 * @@ -128,7 +202,7 @@ static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -138,7 +212,7 @@ static inline uint8_t mavlink_msg_control_status_get_position_fix(const mavlink_ */ static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -148,7 +222,7 @@ static inline uint8_t mavlink_msg_control_status_get_vision_fix(const mavlink_me */ static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -158,7 +232,7 @@ static inline uint8_t mavlink_msg_control_status_get_gps_fix(const mavlink_messa */ static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -168,7 +242,7 @@ static inline uint8_t mavlink_msg_control_status_get_ahrs_health(const mavlink_m */ static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -178,7 +252,7 @@ static inline uint8_t mavlink_msg_control_status_get_control_att(const mavlink_m */ static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -188,7 +262,7 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_xy(const mavlin */ static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -198,7 +272,7 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_z(const mavlink */ static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 7); } /** @@ -209,6 +283,7 @@ static inline uint8_t mavlink_msg_control_status_get_control_pos_yaw(const mavli */ 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); @@ -217,4 +292,7 @@ static inline void mavlink_msg_control_status_decode(const mavlink_message_t* ms 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/libraries/GCS_MAVLink/include/common/mavlink_msg_debug.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_debug.h index e98be847ad..7e80a146b1 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_debug.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_debug.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_DEBUG 255 -typedef struct __mavlink_debug_t +typedef struct __mavlink_debug_t { - uint8_t ind; ///< index of debug variable - float value; ///< DEBUG value - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_debug_t, ind) }, \ + { "value", MAVLINK_TYPE_FLOAT, 0, 1, offsetof(mavlink_debug_t, value) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_debug_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 5); +#else + mavlink_debug_t packet; + packet.ind = ind; + packet.value = value; + + memcpy(_MAV_PAYLOAD(msg), &packet, 5); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG; - - i += put_uint8_t_by_index(ind, i, msg->payload); // index of debug variable - i += put_float_by_index(value, i, msg->payload); // DEBUG value - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 5); } /** - * @brief Pack a debug message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 5); +#else + mavlink_debug_t packet; + packet.ind = ind; + packet.value = value; + + memcpy(_MAV_PAYLOAD(msg), &packet, 5); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG; - - i += put_uint8_t_by_index(ind, i, msg->payload); // index of debug variable - i += put_float_by_index(value, i, msg->payload); // DEBUG value - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { - mavlink_message_t msg; - mavlink_msg_debug_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, ind, value); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, f */ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,12 +146,7 @@ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 1); } /** @@ -118,6 +157,10 @@ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h index 6ea9e0558b..500284e9ba 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_debug_vect.h @@ -2,18 +2,31 @@ #define MAVLINK_MSG_ID_DEBUG_VECT 251 -typedef struct __mavlink_debug_vect_t +typedef struct __mavlink_debug_vect_t { - char name[10]; ///< Name - uint64_t usec; ///< Timestamp - float x; ///< x - float y; ///< y - float z; ///< z - + 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", MAVLINK_TYPE_CHAR, 10, 0, offsetof(mavlink_debug_vect_t, name) }, \ + { "usec", MAVLINK_TYPE_UINT64_T, 0, 10, offsetof(mavlink_debug_vect_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_debug_vect_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 22, offsetof(mavlink_debug_vect_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_debug_vect_t, z) }, \ + } \ +} + /** * @brief Pack a debug_vect message @@ -28,22 +41,33 @@ typedef struct __mavlink_debug_vect_t * @param z z * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) +static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *name, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; +#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(msg), buf, 30); +#else + mavlink_debug_vect_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp - i += put_float_by_index(x, i, msg->payload); // x - i += put_float_by_index(y, i, msg->payload); // y - i += put_float_by_index(z, i, msg->payload); // z - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 30); } /** - * @brief Pack a debug_vect message + * @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 @@ -55,18 +79,30 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co * @param z z * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* name, uint64_t usec, float x, float y, float z) +static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *name,uint64_t usec,float x,float y,float z) { - uint16_t i = 0; +#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(msg), buf, 30); +#else + mavlink_debug_vect_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp - i += put_float_by_index(x, i, msg->payload); // x - i += put_float_by_index(y, i, msg->payload); // y - i += put_float_by_index(z, i, msg->payload); // z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30); } /** @@ -94,26 +130,40 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) +static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char *name, uint64_t usec, float x, float y, float z) { - mavlink_message_t msg; - mavlink_msg_debug_vect_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, usec, x, y, z); - mavlink_send_uart(chan, &msg); +#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; + 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* r_data) +static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char *name) { - - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + return _MAV_RETURN_char_array(msg, name, 10, 0); } /** @@ -123,16 +173,7 @@ static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* */ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(char)*10)[0]; - r.b[6] = (msg->payload+sizeof(char)*10)[1]; - r.b[5] = (msg->payload+sizeof(char)*10)[2]; - r.b[4] = (msg->payload+sizeof(char)*10)[3]; - r.b[3] = (msg->payload+sizeof(char)*10)[4]; - r.b[2] = (msg->payload+sizeof(char)*10)[5]; - r.b[1] = (msg->payload+sizeof(char)*10)[6]; - r.b[0] = (msg->payload+sizeof(char)*10)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 10); } /** @@ -142,12 +183,7 @@ static inline uint64_t mavlink_msg_debug_vect_get_usec(const mavlink_message_t* */ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 18); } /** @@ -157,12 +193,7 @@ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 22); } /** @@ -172,12 +203,7 @@ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(char)*10+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 26); } /** @@ -188,9 +214,13 @@ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h index 9d27f7714b..05cd7456dd 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position.h @@ -2,18 +2,34 @@ #define MAVLINK_MSG_ID_GLOBAL_POSITION 33 -typedef struct __mavlink_global_position_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_t, usec) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_position_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_position_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_t, alt) }, \ + { "vx", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_t, vx) }, \ + { "vy", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_t, vy) }, \ + { "vz", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_t, vz) }, \ + } \ +} /** @@ -31,24 +47,39 @@ typedef struct __mavlink_global_position_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_global_position_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees - i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters - i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) - i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) - i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 32); } /** - * @brief Pack a global_position message + * @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 @@ -62,20 +93,36 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_global_position_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since unix epoch) - i += put_float_by_index(lat, i, msg->payload); // Latitude, in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude, in degrees - i += put_float_by_index(alt, i, msg->payload); // Absolute altitude, in meters - i += put_float_by_index(vx, i, msg->payload); // X Speed (in Latitude direction, positive: going north) - i += put_float_by_index(vy, i, msg->payload); // Y Speed (in Longitude direction, positive: going east) - i += put_float_by_index(vz, i, msg->payload); // Z Speed (in Altitude direction, positive: going up) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); } /** @@ -107,14 +154,36 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin 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) { - mavlink_message_t msg; - mavlink_msg_global_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, lat, lon, alt, vx, vy, vz); - mavlink_send_uart(chan, &msg); +#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 * @@ -122,16 +191,7 @@ static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -141,12 +201,7 @@ static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_messag */ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -156,12 +211,7 @@ static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -171,12 +221,7 @@ static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -186,12 +231,7 @@ static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -201,12 +241,7 @@ static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 24); } /** @@ -216,12 +251,7 @@ static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* */ static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 28); } /** @@ -232,6 +262,7 @@ static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* */ 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); @@ -239,4 +270,7 @@ static inline void mavlink_msg_global_position_decode(const mavlink_message_t* m 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/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h index 1b3277a357..233665115f 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_global_position_int.h @@ -2,17 +2,32 @@ #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 -typedef struct __mavlink_global_position_int_t +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 - + 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", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_global_position_int_t, lat) }, \ + { "lon", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_global_position_int_t, lon) }, \ + { "alt", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_global_position_int_t, alt) }, \ + { "vx", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_global_position_int_t, vx) }, \ + { "vy", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_global_position_int_t, vy) }, \ + { "vz", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_global_position_int_t, vz) }, \ + } \ +} /** @@ -29,23 +44,37 @@ typedef struct __mavlink_global_position_int_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 18); } /** - * @brief Pack a global_position_int message + * @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 @@ -58,19 +87,34 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); } /** @@ -101,14 +145,34 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, 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) { - mavlink_message_t msg; - mavlink_msg_global_position_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, lat, lon, alt, vx, vy, vz); - mavlink_send_uart(chan, &msg); +#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 * @@ -116,12 +180,7 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, */ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -131,12 +190,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -146,12 +200,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -161,10 +210,7 @@ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_mess */ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -174,10 +220,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -187,10 +230,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -201,10 +241,14 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_local_origin_set.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_local_origin_set.h index 1562528541..d71fc7758a 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_local_origin_set.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_local_origin_set.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET 49 -typedef struct __mavlink_gps_local_origin_set_t +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 - + 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", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_local_origin_set_t, latitude) }, \ + { "longitude", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_local_origin_set_t, longitude) }, \ + { "altitude", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_local_origin_set_t, altitude) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_gps_local_origin_set_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_gps_local_origin_set_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - - i += put_int32_t_by_index(latitude, i, msg->payload); // Latitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // Longitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // Altitude(WGS84), expressed as * 1000 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 12); } /** - * @brief Pack a gps_local_origin_set message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_gps_local_origin_set_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - - i += put_int32_t_by_index(latitude, i, msg->payload); // Latitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // Longitude (WGS84), expressed as * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // Altitude(WGS84), expressed as * 1000 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { - mavlink_message_t msg; - mavlink_msg_gps_local_origin_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, latitude, longitude, altitude); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,12 +147,7 @@ static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, */ static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -113,12 +157,7 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_latitude(const mavlin */ static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -128,12 +167,7 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_longitude(const mavli */ static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -144,7 +178,11 @@ static inline int32_t mavlink_msg_gps_local_origin_set_get_altitude(const mavlin */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h index 52d33a23b2..9a91a3e0eb 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_GPS_RAW 32 -typedef struct __mavlink_gps_raw_t +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 - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_t, usec) }, \ + { "fix_type", MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gps_raw_t, fix_type) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_gps_raw_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 13, offsetof(mavlink_gps_raw_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 17, offsetof(mavlink_gps_raw_t, alt) }, \ + { "eph", MAVLINK_TYPE_FLOAT, 0, 21, offsetof(mavlink_gps_raw_t, eph) }, \ + { "epv", MAVLINK_TYPE_FLOAT, 0, 25, offsetof(mavlink_gps_raw_t, epv) }, \ + { "v", MAVLINK_TYPE_FLOAT, 0, 29, offsetof(mavlink_gps_raw_t, v) }, \ + { "hdg", MAVLINK_TYPE_FLOAT, 0, 33, offsetof(mavlink_gps_raw_t, hdg) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_gps_raw_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 37); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees - i += put_float_by_index(alt, i, msg->payload); // Altitude in meters - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 37); } /** - * @brief Pack a gps_raw message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 37); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_float_by_index(lat, i, msg->payload); // Latitude in degrees - i += put_float_by_index(lon, i, msg->payload); // Longitude in degrees - i += put_float_by_index(alt, i, msg->payload); // Altitude in meters - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com 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) { - mavlink_message_t msg; - mavlink_msg_gps_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,16 +213,7 @@ static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t use */ static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -153,7 +223,7 @@ static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + return _MAV_RETURN_uint8_t(msg, 8); } /** @@ -163,12 +233,7 @@ static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* */ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 9); } /** @@ -178,12 +243,7 @@ static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 13); } /** @@ -193,12 +253,7 @@ static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 17); } /** @@ -208,12 +263,7 @@ static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 21); } /** @@ -223,12 +273,7 @@ static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 25); } /** @@ -238,12 +283,7 @@ static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 29); } /** @@ -253,12 +293,7 @@ static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 33); } /** @@ -269,6 +304,7 @@ static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) */ 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); @@ -278,4 +314,7 @@ static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavl 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/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw_int.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw_int.h index b49498febc..9edb55a8f2 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw_int.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_raw_int.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_GPS_RAW_INT 25 -typedef struct __mavlink_gps_raw_int_t +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 - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, usec) }, \ + { "fix_type", MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ + { "lat", MAVLINK_TYPE_INT32_T, 0, 9, offsetof(mavlink_gps_raw_int_t, lat) }, \ + { "lon", MAVLINK_TYPE_INT32_T, 0, 13, offsetof(mavlink_gps_raw_int_t, lon) }, \ + { "alt", MAVLINK_TYPE_INT32_T, 0, 17, offsetof(mavlink_gps_raw_int_t, alt) }, \ + { "eph", MAVLINK_TYPE_FLOAT, 0, 21, offsetof(mavlink_gps_raw_int_t, eph) }, \ + { "epv", MAVLINK_TYPE_FLOAT, 0, 25, offsetof(mavlink_gps_raw_int_t, epv) }, \ + { "v", MAVLINK_TYPE_FLOAT, 0, 29, offsetof(mavlink_gps_raw_int_t, v) }, \ + { "hdg", MAVLINK_TYPE_FLOAT, 0, 33, offsetof(mavlink_gps_raw_int_t, hdg) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_gps_raw_int_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 37); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude in 1E7 degrees - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude in 1E7 degrees - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in 1E3 meters (millimeters) - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed (m/s) - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 37); } /** - * @brief Pack a gps_raw_int message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 37); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_uint8_t_by_index(fix_type, i, msg->payload); // 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. - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude in 1E7 degrees - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude in 1E7 degrees - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in 1E3 meters (millimeters) - i += put_float_by_index(eph, i, msg->payload); // GPS HDOP - i += put_float_by_index(epv, i, msg->payload); // GPS VDOP - i += put_float_by_index(v, i, msg->payload); // GPS ground speed (m/s) - i += put_float_by_index(hdg, i, msg->payload); // Compass heading in degrees, 0..360 degrees - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 37); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t 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) { - mavlink_message_t msg; - mavlink_msg_gps_raw_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,16 +213,7 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_gps_raw_int_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -153,7 +223,7 @@ static inline uint64_t mavlink_msg_gps_raw_int_get_usec(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + return _MAV_RETURN_uint8_t(msg, 8); } /** @@ -163,12 +233,7 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message */ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 9); } /** @@ -178,12 +243,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 13); } /** @@ -193,12 +253,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 17); } /** @@ -208,12 +263,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m */ static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 21); } /** @@ -223,12 +273,7 @@ static inline float mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg */ static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 25); } /** @@ -238,12 +283,7 @@ static inline float mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg */ static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 29); } /** @@ -253,12 +293,7 @@ static inline float mavlink_msg_gps_raw_int_get_v(const mavlink_message_t* msg) */ static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 33); } /** @@ -269,6 +304,7 @@ static inline float mavlink_msg_gps_raw_int_get_hdg(const mavlink_message_t* msg */ 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); @@ -278,4 +314,7 @@ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* 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/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h index 47ddc2e23b..48cd0f42d6 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_set_global_origin.h @@ -2,16 +2,30 @@ #define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 -typedef struct __mavlink_gps_set_global_origin_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_set_global_origin_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_gps_set_global_origin_t, target_component) }, \ + { "latitude", MAVLINK_TYPE_INT32_T, 0, 2, offsetof(mavlink_gps_set_global_origin_t, latitude) }, \ + { "longitude", MAVLINK_TYPE_INT32_T, 0, 6, offsetof(mavlink_gps_set_global_origin_t, longitude) }, \ + { "altitude", MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_gps_set_global_origin_t, altitude) }, \ + } \ +} /** @@ -27,22 +41,35 @@ typedef struct __mavlink_gps_set_global_origin_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_int32_t_by_index(latitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // global position * 1000 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 14); } /** - * @brief Pack a gps_set_global_origin message + * @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 @@ -54,18 +81,32 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_int32_t_by_index(latitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(longitude, i, msg->payload); // global position * 1E7 - i += put_int32_t_by_index(altitude, i, msg->payload); // global position * 1000 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); } /** @@ -95,14 +136,32 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i 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) { - mavlink_message_t msg; - mavlink_msg_gps_set_global_origin_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, latitude, longitude, altitude); - mavlink_send_uart(chan, &msg); +#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 * @@ -110,7 +169,7 @@ static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +179,7 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const */ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -130,12 +189,7 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(con */ static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 2); } /** @@ -145,12 +199,7 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavli */ static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 6); } /** @@ -160,12 +209,7 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavl */ static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 10); } /** @@ -176,9 +220,13 @@ static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavli */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_status.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_status.h index fd04a9a262..6d415dbd03 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_status.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_gps_status.h @@ -2,23 +2,37 @@ #define MAVLINK_MSG_ID_GPS_STATUS 27 -typedef struct __mavlink_gps_status_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_gps_status_t, satellites_visible) }, \ + { "satellite_prn", MAVLINK_TYPE_INT8_T, 20, 1, offsetof(mavlink_gps_status_t, satellite_prn) }, \ + { "satellite_used", MAVLINK_TYPE_INT8_T, 20, 21, offsetof(mavlink_gps_status_t, satellite_used) }, \ + { "satellite_elevation", MAVLINK_TYPE_INT8_T, 20, 41, offsetof(mavlink_gps_status_t, satellite_elevation) }, \ + { "satellite_azimuth", MAVLINK_TYPE_INT8_T, 20, 61, offsetof(mavlink_gps_status_t, satellite_azimuth) }, \ + { "satellite_snr", MAVLINK_TYPE_INT8_T, 20, 81, offsetof(mavlink_gps_status_t, satellite_snr) }, \ + } \ +} + /** * @brief Pack a gps_status message @@ -34,23 +48,35 @@ typedef struct __mavlink_gps_status_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + memcpy(packet.satellite_prn, satellite_prn, sizeof(int8_t)*20); + memcpy(packet.satellite_used, satellite_used, sizeof(int8_t)*20); + memcpy(packet.satellite_elevation, satellite_elevation, sizeof(int8_t)*20); + memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(int8_t)*20); + memcpy(packet.satellite_snr, satellite_snr, sizeof(int8_t)*20); + memcpy(_MAV_PAYLOAD(msg), &packet, 101); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - - i += put_uint8_t_by_index(satellites_visible, i, msg->payload); // Number of satellites visible - i += put_array_by_index(satellite_prn, 20, i, msg->payload); // Global satellite ID - i += put_array_by_index(satellite_used, 20, i, msg->payload); // 0: Satellite not used, 1: used for localization - i += put_array_by_index(satellite_elevation, 20, i, msg->payload); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - i += put_array_by_index(satellite_azimuth, 20, i, msg->payload); // Direction of satellite, 0: 0 deg, 255: 360 deg. - i += put_array_by_index(satellite_snr, 20, i, msg->payload); // Signal to noise ratio of satellite - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 101); } /** - * @brief Pack a gps_status message + * @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 @@ -63,19 +89,32 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + memcpy(packet.satellite_prn, satellite_prn, sizeof(int8_t)*20); + memcpy(packet.satellite_used, satellite_used, sizeof(int8_t)*20); + memcpy(packet.satellite_elevation, satellite_elevation, sizeof(int8_t)*20); + memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(int8_t)*20); + memcpy(packet.satellite_snr, satellite_snr, sizeof(int8_t)*20); + memcpy(_MAV_PAYLOAD(msg), &packet, 101); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - - i += put_uint8_t_by_index(satellites_visible, i, msg->payload); // Number of satellites visible - i += put_array_by_index(satellite_prn, 20, i, msg->payload); // Global satellite ID - i += put_array_by_index(satellite_used, 20, i, msg->payload); // 0: Satellite not used, 1: used for localization - i += put_array_by_index(satellite_elevation, 20, i, msg->payload); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - i += put_array_by_index(satellite_azimuth, 20, i, msg->payload); // Direction of satellite, 0: 0 deg, 255: 360 deg. - i += put_array_by_index(satellite_snr, 20, i, msg->payload); // Signal to noise ratio of satellite - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 101); } /** @@ -104,16 +143,34 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t */ #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) +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) { - mavlink_message_t msg; - mavlink_msg_gps_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr); - mavlink_send_uart(chan, &msg); +#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; + memcpy(packet.satellite_prn, satellite_prn, sizeof(int8_t)*20); + memcpy(packet.satellite_used, satellite_used, sizeof(int8_t)*20); + memcpy(packet.satellite_elevation, satellite_elevation, sizeof(int8_t)*20); + memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(int8_t)*20); + 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 * @@ -121,7 +178,7 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -129,11 +186,9 @@ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlin * * @return Global satellite ID */ -static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, int8_t *satellite_prn) { - - memcpy(r_data, msg->payload+sizeof(uint8_t), 20); - return 20; + return _MAV_RETURN_int8_t_array(msg, satellite_prn, 20, 1); } /** @@ -141,11 +196,9 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_me * * @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* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, int8_t *satellite_used) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+20, 20); - return 20; + return _MAV_RETURN_int8_t_array(msg, satellite_used, 20, 21); } /** @@ -153,11 +206,9 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_m * * @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* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, int8_t *satellite_elevation) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20, 20); - return 20; + return _MAV_RETURN_int8_t_array(msg, satellite_elevation, 20, 41); } /** @@ -165,11 +216,9 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavl * * @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* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, int8_t *satellite_azimuth) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20, 20); - return 20; + return _MAV_RETURN_int8_t_array(msg, satellite_azimuth, 20, 61); } /** @@ -177,11 +226,9 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlin * * @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* r_data) +static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, int8_t *satellite_snr) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20+20, 20); - return 20; + return _MAV_RETURN_int8_t_array(msg, satellite_snr, 20, 81); } /** @@ -192,10 +239,14 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_me */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_heartbeat.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_heartbeat.h index 0e5c4db5ca..4006cd5585 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_heartbeat.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_heartbeat.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_HEARTBEAT 0 -typedef struct __mavlink_heartbeat_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_heartbeat_t, type) }, \ + { "autopilot", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_heartbeat_t, autopilot) }, \ + { "mavlink_version", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_heartbeat_t, mavlink_version) }, \ + } \ +} /** @@ -22,20 +34,31 @@ typedef struct __mavlink_heartbeat_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 3); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mavlink_version = 2; + + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 3); } /** - * @brief Pack a heartbeat message + * @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 @@ -44,16 +67,28 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 3); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mavlink_version = 2; + + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); } /** @@ -80,14 +115,28 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { - mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); +#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 * @@ -95,7 +144,7 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -105,7 +154,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -115,7 +164,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -126,7 +175,11 @@ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_me */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_controls.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_controls.h index 80f33ebd06..6f43ff26ac 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_controls.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_controls.h @@ -2,18 +2,34 @@ #define MAVLINK_MSG_ID_HIL_CONTROLS 68 -typedef struct __mavlink_hil_controls_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_controls_t, time_us) }, \ + { "roll_ailerons", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_controls_t, roll_ailerons) }, \ + { "pitch_elevator", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_controls_t, pitch_elevator) }, \ + { "yaw_rudder", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_controls_t, yaw_rudder) }, \ + { "throttle", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_controls_t, throttle) }, \ + { "mode", MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_hil_controls_t, mode) }, \ + { "nav_mode", MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_hil_controls_t, nav_mode) }, \ + } \ +} /** @@ -31,24 +47,39 @@ typedef struct __mavlink_hil_controls_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - - i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1 - i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1 - i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1 - i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1 - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE) - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 26); } /** - * @brief Pack a hil_controls message + * @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 @@ -62,20 +93,36 @@ static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - - i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1 - i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1 - i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1 - i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1 - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE) - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); } /** @@ -107,14 +154,36 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ 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) { - mavlink_message_t msg; - mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode); - mavlink_send_uart(chan, &msg); +#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 * @@ -122,16 +191,7 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -141,12 +201,7 @@ static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_messag */ static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -156,12 +211,7 @@ static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_mes */ static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -171,12 +221,7 @@ static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_me */ static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -186,12 +231,7 @@ static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_messag */ static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -201,7 +241,7 @@ static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_ */ static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + return _MAV_RETURN_uint8_t(msg, 24); } /** @@ -211,7 +251,7 @@ static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* */ static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 25); } /** @@ -222,6 +262,7 @@ static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_messag */ 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); @@ -229,4 +270,7 @@ static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* 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/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_state.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_state.h index 371cd4ae46..9f6af9c0cb 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_state.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_hil_state.h @@ -2,27 +2,52 @@ #define MAVLINK_MSG_ID_HIL_STATE 67 -typedef struct __mavlink_hil_state_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_state_t, usec) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_state_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_state_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_hil_state_t, yaw) }, \ + { "rollspeed", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_hil_state_t, rollspeed) }, \ + { "pitchspeed", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_hil_state_t, pitchspeed) }, \ + { "yawspeed", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_hil_state_t, yawspeed) }, \ + { "lat", MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_hil_state_t, lat) }, \ + { "lon", MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_hil_state_t, lon) }, \ + { "alt", MAVLINK_TYPE_INT32_T, 0, 40, offsetof(mavlink_hil_state_t, alt) }, \ + { "vx", MAVLINK_TYPE_INT16_T, 0, 44, offsetof(mavlink_hil_state_t, vx) }, \ + { "vy", MAVLINK_TYPE_INT16_T, 0, 46, offsetof(mavlink_hil_state_t, vy) }, \ + { "vz", MAVLINK_TYPE_INT16_T, 0, 48, offsetof(mavlink_hil_state_t, vz) }, \ + { "xacc", MAVLINK_TYPE_INT16_T, 0, 50, offsetof(mavlink_hil_state_t, xacc) }, \ + { "yacc", MAVLINK_TYPE_INT16_T, 0, 52, offsetof(mavlink_hil_state_t, yacc) }, \ + { "zacc", MAVLINK_TYPE_INT16_T, 0, 54, offsetof(mavlink_hil_state_t, zacc) }, \ + } \ +} /** @@ -49,33 +74,57 @@ typedef struct __mavlink_hil_state_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 56); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 56); } /** - * @brief Pack a hil_state message + * @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 @@ -98,29 +147,54 @@ static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t com * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 56); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad) - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad) - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad) - i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s) - i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s) - i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s) - i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7 - i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7 - i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters) - i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100 - i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100 - i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100 - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 56); } /** @@ -161,14 +235,54 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c 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) { - mavlink_message_t msg; - mavlink_msg_hil_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); - mavlink_send_uart(chan, &msg); +#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 * @@ -176,16 +290,7 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t u */ static inline uint64_t mavlink_msg_hil_state_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -195,12 +300,7 @@ static inline uint64_t mavlink_msg_hil_state_get_usec(const mavlink_message_t* m */ static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -210,12 +310,7 @@ static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -225,12 +320,7 @@ static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg */ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -240,12 +330,7 @@ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -255,12 +340,7 @@ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 24); } /** @@ -270,12 +350,7 @@ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t */ static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 28); } /** @@ -285,12 +360,7 @@ static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* */ static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 32); } /** @@ -300,12 +370,7 @@ static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg */ static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 36); } /** @@ -315,12 +380,7 @@ static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg */ static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 40); } /** @@ -330,10 +390,7 @@ static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 44); } /** @@ -343,10 +400,7 @@ static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 46); } /** @@ -356,10 +410,7 @@ static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 48); } /** @@ -369,10 +420,7 @@ static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 50); } /** @@ -382,10 +430,7 @@ static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* ms */ static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 52); } /** @@ -395,10 +440,7 @@ static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* ms */ static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 54); } /** @@ -409,6 +451,7 @@ static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* ms */ 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); @@ -425,4 +468,7 @@ static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, ma 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/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position.h index c9cb453c0e..1adbd0c540 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position.h @@ -2,18 +2,34 @@ #define MAVLINK_MSG_ID_LOCAL_POSITION 31 -typedef struct __mavlink_local_position_t +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 - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_local_position_t, usec) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_local_position_t, z) }, \ + { "vx", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_local_position_t, vx) }, \ + { "vy", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_local_position_t, vy) }, \ + { "vz", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_local_position_t, vz) }, \ + } \ +} /** @@ -31,24 +47,39 @@ typedef struct __mavlink_local_position_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_local_position_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 32); } /** - * @brief Pack a local_position message + * @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 @@ -62,20 +93,36 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_local_position_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(x, i, msg->payload); // X Position - i += put_float_by_index(y, i, msg->payload); // Y Position - i += put_float_by_index(z, i, msg->payload); // Z Position - i += put_float_by_index(vx, i, msg->payload); // X Speed - i += put_float_by_index(vy, i, msg->payload); // Y Speed - i += put_float_by_index(vz, i, msg->payload); // Z Speed - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32); } /** @@ -107,14 +154,36 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint 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) { - mavlink_message_t msg; - mavlink_msg_local_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, vx, vy, vz); - mavlink_send_uart(chan, &msg); +#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 * @@ -122,16 +191,7 @@ static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint6 */ static inline uint64_t mavlink_msg_local_position_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -141,12 +201,7 @@ static inline uint64_t mavlink_msg_local_position_get_usec(const mavlink_message */ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -156,12 +211,7 @@ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -171,12 +221,7 @@ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -186,12 +231,7 @@ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -201,12 +241,7 @@ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 24); } /** @@ -216,12 +251,7 @@ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 28); } /** @@ -232,6 +262,7 @@ static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* m */ 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); @@ -239,4 +270,7 @@ static inline void mavlink_msg_local_position_decode(const mavlink_message_t* ms 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/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint.h index fcf4733476..0bc4d98007 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint.h @@ -2,15 +2,28 @@ #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 -typedef struct __mavlink_local_position_setpoint_t +typedef struct __mavlink_local_position_setpoint_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - + 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", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_local_position_setpoint_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_local_position_setpoint_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_local_position_setpoint_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_local_position_setpoint_t, yaw) }, \ + } \ +} /** @@ -25,21 +38,33 @@ typedef struct __mavlink_local_position_setpoint_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 16); } /** - * @brief Pack a local_position_setpoint message + * @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 @@ -50,17 +75,30 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); } /** @@ -89,14 +127,30 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_local_position_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, yaw); - mavlink_send_uart(chan, &msg); +#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 * @@ -104,12 +158,7 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch */ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 0); } /** @@ -119,12 +168,7 @@ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -134,12 +178,7 @@ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -149,12 +188,7 @@ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -165,8 +199,12 @@ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_me */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint_set.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint_set.h index f3383287eb..9eea4d7195 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint_set.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_local_position_setpoint_set.h @@ -2,17 +2,32 @@ #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 -typedef struct __mavlink_local_position_setpoint_set_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_local_position_setpoint_set_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_local_position_setpoint_set_t, target_component) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_local_position_setpoint_set_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_local_position_setpoint_set_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_local_position_setpoint_set_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_local_position_setpoint_set_t, yaw) }, \ + } \ +} /** @@ -29,23 +44,37 @@ typedef struct __mavlink_local_position_setpoint_set_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 18); } /** - * @brief Pack a local_position_setpoint_set message + * @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 @@ -58,19 +87,34 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); } /** @@ -101,14 +145,34 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy 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) { - mavlink_message_t msg; - mavlink_msg_local_position_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); +#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 * @@ -116,7 +180,7 @@ static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -126,7 +190,7 @@ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_system( */ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -136,12 +200,7 @@ static inline uint8_t mavlink_msg_local_position_setpoint_set_get_target_compone */ static inline float mavlink_msg_local_position_setpoint_set_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 2); } /** @@ -151,12 +210,7 @@ static inline float mavlink_msg_local_position_setpoint_set_get_x(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 6); } /** @@ -166,12 +220,7 @@ static inline float mavlink_msg_local_position_setpoint_set_get_y(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 10); } /** @@ -181,12 +230,7 @@ static inline float mavlink_msg_local_position_setpoint_set_get_z(const mavlink_ */ static inline float mavlink_msg_local_position_setpoint_set_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 14); } /** @@ -197,10 +241,14 @@ static inline float mavlink_msg_local_position_setpoint_set_get_yaw(const mavlin */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_manual_control.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_manual_control.h index deec098f63..8b2e24d43a 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_manual_control.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_manual_control.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_MANUAL_CONTROL 69 -typedef struct __mavlink_manual_control_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_manual_control_t, target) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 1, offsetof(mavlink_manual_control_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_manual_control_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_manual_control_t, yaw) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 13, offsetof(mavlink_manual_control_t, thrust) }, \ + { "roll_manual", MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_manual_control_t, roll_manual) }, \ + { "pitch_manual", MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_manual_control_t, pitch_manual) }, \ + { "yaw_manual", MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_manual_control_t, yaw_manual) }, \ + { "thrust_manual", MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_manual_control_t, thrust_manual) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_manual_control_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 21); } /** - * @brief Pack a manual_control message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system to be controlled - i += put_float_by_index(roll, i, msg->payload); // roll - i += put_float_by_index(pitch, i, msg->payload); // pitch - i += put_float_by_index(yaw, i, msg->payload); // yaw - i += put_float_by_index(thrust, i, msg->payload); // thrust - i += put_uint8_t_by_index(roll_manual, i, msg->payload); // roll control enabled auto:0, manual:1 - i += put_uint8_t_by_index(pitch_manual, i, msg->payload); // pitch auto:0, manual:1 - i += put_uint8_t_by_index(yaw_manual, i, msg->payload); // yaw auto:0, manual:1 - i += put_uint8_t_by_index(thrust_manual, i, msg->payload); // thrust auto:0, manual:1 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint 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) { - mavlink_message_t msg; - mavlink_msg_manual_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,7 +213,7 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -144,12 +223,7 @@ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_messag */ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 1); } /** @@ -159,12 +233,7 @@ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 5); } /** @@ -174,12 +243,7 @@ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t */ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 9); } /** @@ -189,12 +253,7 @@ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 13); } /** @@ -204,7 +263,7 @@ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_ */ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -214,7 +273,7 @@ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_m */ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 18); } /** @@ -224,7 +283,7 @@ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_ */ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 19); } /** @@ -234,7 +293,7 @@ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_me */ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -245,6 +304,7 @@ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink */ 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); @@ -254,4 +314,7 @@ static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* ms 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/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_float.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_float.h index aaff215ce8..791e32421c 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_float.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_float.h @@ -2,15 +2,25 @@ #define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 252 -typedef struct __mavlink_named_value_float_t +typedef struct __mavlink_named_value_float_t { - char name[10]; ///< Name of the debug variable - float value; ///< Floating point value - + 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", MAVLINK_TYPE_CHAR, 10, 0, offsetof(mavlink_named_value_float_t, name) }, \ + { "value", MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_named_value_float_t, value) }, \ + } \ +} + /** * @brief Pack a named_value_float message @@ -22,19 +32,27 @@ typedef struct __mavlink_named_value_float_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 14); +#else + mavlink_named_value_float_t packet; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_float_by_index(value, i, msg->payload); // Floating point value - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 14); } /** - * @brief Pack a named_value_float message + * @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 @@ -43,15 +61,24 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 14); +#else + mavlink_named_value_float_t packet; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_float_by_index(value, i, msg->payload); // Floating point value - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); } /** @@ -76,26 +103,34 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) +static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char *name, float value) { - mavlink_message_t msg; - mavlink_msg_named_value_float_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, value); - mavlink_send_uart(chan, &msg); +#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; + 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* r_data) +static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char *name) { - - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + return _MAV_RETURN_char_array(msg, name, 10, 0); } /** @@ -105,12 +140,7 @@ static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_mess */ static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10)[0]; - r.b[2] = (msg->payload+sizeof(char)*10)[1]; - r.b[1] = (msg->payload+sizeof(char)*10)[2]; - r.b[0] = (msg->payload+sizeof(char)*10)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 10); } /** @@ -121,6 +151,10 @@ static inline float mavlink_msg_named_value_float_get_value(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_int.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_int.h index 92bf5fcb2a..18bad80a11 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_int.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_named_value_int.h @@ -2,15 +2,25 @@ #define MAVLINK_MSG_ID_NAMED_VALUE_INT 253 -typedef struct __mavlink_named_value_int_t +typedef struct __mavlink_named_value_int_t { - char name[10]; ///< Name of the debug variable - int32_t value; ///< Signed integer value - + 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", MAVLINK_TYPE_CHAR, 10, 0, offsetof(mavlink_named_value_int_t, name) }, \ + { "value", MAVLINK_TYPE_INT32_T, 0, 10, offsetof(mavlink_named_value_int_t, value) }, \ + } \ +} + /** * @brief Pack a named_value_int message @@ -22,19 +32,27 @@ typedef struct __mavlink_named_value_int_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 14); +#else + mavlink_named_value_int_t packet; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_int32_t_by_index(value, i, msg->payload); // Signed integer value - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 14); } /** - * @brief Pack a named_value_int message + * @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 @@ -43,15 +61,24 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 14); +#else + mavlink_named_value_int_t packet; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - - i += put_array_by_index((const int8_t*)name, sizeof(char)*10, i, msg->payload); // Name of the debug variable - i += put_int32_t_by_index(value, i, msg->payload); // Signed integer value - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); } /** @@ -76,26 +103,34 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) +static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char *name, int32_t value) { - mavlink_message_t msg; - mavlink_msg_named_value_int_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, name, value); - mavlink_send_uart(chan, &msg); +#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; + 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* r_data) +static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char *name) { - - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + return _MAV_RETURN_char_array(msg, name, 10, 0); } /** @@ -105,12 +140,7 @@ static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_messag */ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(char)*10)[0]; - r.b[2] = (msg->payload+sizeof(char)*10)[1]; - r.b[1] = (msg->payload+sizeof(char)*10)[2]; - r.b[0] = (msg->payload+sizeof(char)*10)[3]; - return (int32_t)r.i; + return _MAV_RETURN_int32_t(msg, 10); } /** @@ -121,6 +151,10 @@ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h index 8d0b81eb87..34cdb4b671 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_nav_controller_output.h @@ -2,19 +2,36 @@ #define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 -typedef struct __mavlink_nav_controller_output_t +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 - + 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", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_nav_controller_output_t, nav_roll) }, \ + { "nav_pitch", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_nav_controller_output_t, nav_pitch) }, \ + { "nav_bearing", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_nav_controller_output_t, nav_bearing) }, \ + { "target_bearing", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_nav_controller_output_t, target_bearing) }, \ + { "wp_dist", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_nav_controller_output_t, wp_dist) }, \ + { "alt_error", MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_nav_controller_output_t, alt_error) }, \ + { "aspd_error", MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_nav_controller_output_t, aspd_error) }, \ + { "xtrack_error", MAVLINK_TYPE_FLOAT, 0, 22, offsetof(mavlink_nav_controller_output_t, xtrack_error) }, \ + } \ +} /** @@ -33,25 +50,41 @@ typedef struct __mavlink_nav_controller_output_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 26); } /** - * @brief Pack a nav_controller_output message + * @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 @@ -66,21 +99,38 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); } /** @@ -113,14 +163,38 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i 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) { - mavlink_message_t msg; - mavlink_msg_nav_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error); - mavlink_send_uart(chan, &msg); +#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 * @@ -128,12 +202,7 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan */ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 0); } /** @@ -143,12 +212,7 @@ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink */ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -158,10 +222,7 @@ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlin */ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -171,10 +232,7 @@ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const ma */ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -184,10 +242,7 @@ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const */ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -197,12 +252,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavli */ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 14); } /** @@ -212,12 +262,7 @@ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlin */ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 18); } /** @@ -227,12 +272,7 @@ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavli */ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 22); } /** @@ -243,6 +283,7 @@ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mav */ 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); @@ -251,4 +292,7 @@ static inline void mavlink_msg_nav_controller_output_decode(const mavlink_messag 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/libraries/GCS_MAVLink/include/common/mavlink_msg_object_detection_event.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_object_detection_event.h new file mode 100644 index 0000000000..ac53aab00b --- /dev/null +++ b/libraries/GCS_MAVLink/include/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", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_object_detection_event_t, time) }, \ + { "object_id", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_object_detection_event_t, object_id) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_object_detection_event_t, type) }, \ + { "name", MAVLINK_TYPE_CHAR, 20, 7, offsetof(mavlink_object_detection_event_t, name) }, \ + { "quality", MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_object_detection_event_t, quality) }, \ + { "bearing", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_object_detection_event_t, bearing) }, \ + { "distance", 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(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; + memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + return mavlink_finalize_message(msg, system_id, component_id, 36); +} + +/** + * @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(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; + memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36); +} + +/** + * @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; + 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/libraries/GCS_MAVLink/include/common/mavlink_msg_optical_flow.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_optical_flow.h new file mode 100644 index 0000000000..00f1c4c237 --- /dev/null +++ b/libraries/GCS_MAVLink/include/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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time) }, \ + { "sensor_id", MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_optical_flow_t, sensor_id) }, \ + { "flow_x", MAVLINK_TYPE_INT16_T, 0, 9, offsetof(mavlink_optical_flow_t, flow_x) }, \ + { "flow_y", MAVLINK_TYPE_INT16_T, 0, 11, offsetof(mavlink_optical_flow_t, flow_y) }, \ + { "quality", MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_optical_flow_t, quality) }, \ + { "ground_distance", 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(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(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(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(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/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_list.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_list.h index e4912fb591..817f50a3f3 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_list.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_list.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 -typedef struct __mavlink_param_request_list_t +typedef struct __mavlink_param_request_list_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_list_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_list_t, target_component) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_param_request_list_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a param_request_list message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_message_t msg; - mavlink_msg_param_request_list_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,7 +146,7 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -113,6 +157,10 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_read.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_read.h index 2cd9d2932e..93db0b610e 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_read.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_request_read.h @@ -2,17 +2,29 @@ #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 -typedef struct __mavlink_param_request_read_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_request_read_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_request_read_t, target_component) }, \ + { "param_id", MAVLINK_TYPE_INT8_T, 15, 2, offsetof(mavlink_param_request_read_t, param_id) }, \ + { "param_index", MAVLINK_TYPE_INT16_T, 0, 17, offsetof(mavlink_param_request_read_t, param_index) }, \ + } \ +} + /** * @brief Pack a param_request_read message @@ -26,21 +38,31 @@ typedef struct __mavlink_param_request_read_t * @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) +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) { - uint16_t i = 0; +#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(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; + memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD(msg), &packet, 19); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_int16_t_by_index(param_index, i, msg->payload); // Parameter index. Send -1 to use the param ID field as identifier - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 19); } /** - * @brief Pack a param_request_read message + * @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 @@ -51,17 +73,28 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @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) +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) { - uint16_t i = 0; +#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(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; + memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD(msg), &packet, 19); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_int16_t_by_index(param_index, i, msg->payload); // Parameter index. Send -1 to use the param ID field as identifier - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 19); } /** @@ -88,16 +121,30 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, */ #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) +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) { - mavlink_message_t msg; - mavlink_msg_param_request_read_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, param_id, param_index); - mavlink_send_uart(chan, &msg); +#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; + 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 * @@ -105,7 +152,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -115,7 +162,7 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -123,11 +170,9 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, int8_t *param_id) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + return _MAV_RETURN_int8_t_array(msg, param_id, 15, 2); } /** @@ -137,10 +182,7 @@ static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink */ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 17); } /** @@ -151,8 +193,12 @@ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavli */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_param_set.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_set.h index de1c314022..e54b9ba38a 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_param_set.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_set.h @@ -2,17 +2,29 @@ #define MAVLINK_MSG_ID_PARAM_SET 23 -typedef struct __mavlink_param_set_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_set_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_set_t, target_component) }, \ + { "param_id", MAVLINK_TYPE_INT8_T, 15, 2, offsetof(mavlink_param_set_t, param_id) }, \ + { "param_value", MAVLINK_TYPE_FLOAT, 0, 17, offsetof(mavlink_param_set_t, param_value) }, \ + } \ +} + /** * @brief Pack a param_set message @@ -26,21 +38,31 @@ typedef struct __mavlink_param_set_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 21); +#else + mavlink_param_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_value = param_value; + memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 21); } /** - * @brief Pack a param_set message + * @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 @@ -51,17 +73,28 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 21); +#else + mavlink_param_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_value = param_value; + memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21); } /** @@ -88,16 +121,30 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c */ #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) +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) { - mavlink_message_t msg; - mavlink_msg_param_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, param_id, param_value); - mavlink_send_uart(chan, &msg); +#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; + 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 * @@ -105,7 +152,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta */ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -115,7 +162,7 @@ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_mess */ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -123,11 +170,9 @@ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_m * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, int8_t *param_id) { - - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + return _MAV_RETURN_int8_t_array(msg, param_id, 15, 2); } /** @@ -137,12 +182,7 @@ static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_ */ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+15)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 17); } /** @@ -153,8 +193,12 @@ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_ */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_param_value.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_value.h index 239c96c42f..84b4e7dd16 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_param_value.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_param_value.h @@ -2,17 +2,29 @@ #define MAVLINK_MSG_ID_PARAM_VALUE 22 -typedef struct __mavlink_param_value_t +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 - + 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", MAVLINK_TYPE_INT8_T, 15, 0, offsetof(mavlink_param_value_t, param_id) }, \ + { "param_value", MAVLINK_TYPE_FLOAT, 0, 15, offsetof(mavlink_param_value_t, param_value) }, \ + { "param_count", MAVLINK_TYPE_UINT16_T, 0, 19, offsetof(mavlink_param_value_t, param_count) }, \ + { "param_index", MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_param_value_t, param_index) }, \ + } \ +} + /** * @brief Pack a param_value message @@ -26,21 +38,31 @@ typedef struct __mavlink_param_value_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 23); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD(msg), &packet, 23); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - i += put_uint16_t_by_index(param_count, i, msg->payload); // Total number of onboard parameters - i += put_uint16_t_by_index(param_index, i, msg->payload); // Index of this onboard parameter - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 23); } /** - * @brief Pack a param_value message + * @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 @@ -51,17 +73,28 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 23); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + memcpy(packet.param_id, param_id, sizeof(int8_t)*15); + memcpy(_MAV_PAYLOAD(msg), &packet, 23); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - - i += put_array_by_index(param_id, 15, i, msg->payload); // Onboard parameter id - i += put_float_by_index(param_value, i, msg->payload); // Onboard parameter value - i += put_uint16_t_by_index(param_count, i, msg->payload); // Total number of onboard parameters - i += put_uint16_t_by_index(param_index, i, msg->payload); // Index of this onboard parameter - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23); } /** @@ -88,26 +121,38 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t */ #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) +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) { - mavlink_message_t msg; - mavlink_msg_param_value_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, param_id, param_value, param_count, param_index); - mavlink_send_uart(chan, &msg); +#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; + 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* r_data) +static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, int8_t *param_id) { - - memcpy(r_data, msg->payload, 15); - return 15; + return _MAV_RETURN_int8_t_array(msg, param_id, 15, 0); } /** @@ -117,12 +162,7 @@ static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_messag */ static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+15)[0]; - r.b[2] = (msg->payload+15)[1]; - r.b[1] = (msg->payload+15)[2]; - r.b[0] = (msg->payload+15)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 15); } /** @@ -132,10 +172,7 @@ static inline float mavlink_msg_param_value_get_param_value(const mavlink_messag */ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+15+sizeof(float))[0]; - r.b[0] = (msg->payload+15+sizeof(float))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 19); } /** @@ -145,10 +182,7 @@ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_mes */ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+15+sizeof(float)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+15+sizeof(float)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 21); } /** @@ -159,8 +193,12 @@ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_mes */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_ping.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_ping.h index 2a27b1e5cb..536c7f1d58 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_ping.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_ping.h @@ -2,15 +2,28 @@ #define MAVLINK_MSG_ID_PING 3 -typedef struct __mavlink_ping_t +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 - + 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", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ping_t, seq) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_ping_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_ping_t, target_component) }, \ + { "time", MAVLINK_TYPE_UINT64_T, 0, 6, offsetof(mavlink_ping_t, time) }, \ + } \ +} /** @@ -25,21 +38,33 @@ typedef struct __mavlink_ping_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_PING; - - i += put_uint32_t_by_index(seq, i, msg->payload); // PING sequence - i += put_uint8_t_by_index(target_system, i, msg->payload); // 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 - i += put_uint8_t_by_index(target_component, i, msg->payload); // 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 - i += put_uint64_t_by_index(time, i, msg->payload); // Unix timestamp in microseconds - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 14); } /** - * @brief Pack a ping message + * @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 @@ -50,17 +75,30 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_PING; - - i += put_uint32_t_by_index(seq, i, msg->payload); // PING sequence - i += put_uint8_t_by_index(target_system, i, msg->payload); // 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 - i += put_uint8_t_by_index(target_component, i, msg->payload); // 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 - i += put_uint64_t_by_index(time, i, msg->payload); // Unix timestamp in microseconds - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14); } /** @@ -89,14 +127,30 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon 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) { - mavlink_message_t msg; - mavlink_msg_ping_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq, target_system, target_component, time); - mavlink_send_uart(chan, &msg); +#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 * @@ -104,12 +158,7 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, u */ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -119,7 +168,7 @@ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) */ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint32_t))[0]; + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -129,7 +178,7 @@ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t */ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -139,16 +188,7 @@ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_messag */ static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 6); } /** @@ -159,8 +199,12 @@ static inline uint64_t mavlink_msg_ping_get_time(const mavlink_message_t* msg) */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_position_target.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_position_target.h index d6e4da3dd4..42998e267d 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_position_target.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_position_target.h @@ -2,15 +2,28 @@ #define MAVLINK_MSG_ID_POSITION_TARGET 63 -typedef struct __mavlink_position_target_t +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 - + 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", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_target_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_t, yaw) }, \ + } \ +} /** @@ -25,21 +38,33 @@ typedef struct __mavlink_position_target_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 16); } /** - * @brief Pack a position_target message + * @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 @@ -50,17 +75,30 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - - i += put_float_by_index(x, i, msg->payload); // x position - i += put_float_by_index(y, i, msg->payload); // y position - i += put_float_by_index(z, i, msg->payload); // z position - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation in radians, 0 = NORTH - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); } /** @@ -89,14 +127,30 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - mavlink_message_t msg; - mavlink_msg_position_target_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, yaw); - mavlink_send_uart(chan, &msg); +#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 * @@ -104,12 +158,7 @@ static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, floa */ static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 0); } /** @@ -119,12 +168,7 @@ static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -134,12 +178,7 @@ static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -149,12 +188,7 @@ static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* m */ static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -165,8 +199,12 @@ static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_imu.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_imu.h index 4ded0b7119..f31a232990 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_imu.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_imu.h @@ -2,21 +2,40 @@ #define MAVLINK_MSG_ID_RAW_IMU 28 -typedef struct __mavlink_raw_imu_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, usec) }, \ + { "xacc", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ + { "yacc", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ + { "zacc", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_imu_t, zacc) }, \ + { "xgyro", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_imu_t, xgyro) }, \ + { "ygyro", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_raw_imu_t, ygyro) }, \ + { "zgyro", MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_raw_imu_t, zgyro) }, \ + { "xmag", MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ + { "ymag", MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ + { "zmag", MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + } \ +} /** @@ -37,27 +56,45 @@ typedef struct __mavlink_raw_imu_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (raw) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (raw) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (raw) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (raw) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (raw) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (raw) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (raw) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (raw) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (raw) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 26); } /** - * @brief Pack a raw_imu message + * @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 @@ -74,23 +111,42 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (raw) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (raw) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (raw) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (raw) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (raw) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (raw) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (raw) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (raw) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (raw) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); } /** @@ -125,14 +181,42 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com 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) { - mavlink_message_t msg; - mavlink_msg_raw_imu_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); - mavlink_send_uart(chan, &msg); +#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 * @@ -140,16 +224,7 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t use */ static inline uint64_t mavlink_msg_raw_imu_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -159,10 +234,7 @@ static inline uint64_t mavlink_msg_raw_imu_get_usec(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -172,10 +244,7 @@ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -185,10 +254,7 @@ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -198,10 +264,7 @@ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -211,10 +274,7 @@ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -224,10 +284,7 @@ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 18); } /** @@ -237,10 +294,7 @@ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 20); } /** @@ -250,10 +304,7 @@ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 22); } /** @@ -263,10 +314,7 @@ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 24); } /** @@ -277,6 +325,7 @@ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) */ 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); @@ -287,4 +336,7 @@ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavl 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/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_pressure.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_pressure.h index 8346ac6286..598e3c2059 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_pressure.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_raw_pressure.h @@ -2,16 +2,30 @@ #define MAVLINK_MSG_ID_RAW_PRESSURE 29 -typedef struct __mavlink_raw_pressure_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_pressure_t, usec) }, \ + { "press_abs", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_pressure_t, press_abs) }, \ + { "press_diff1", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_pressure_t, press_diff1) }, \ + { "press_diff2", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_raw_pressure_t, press_diff2) }, \ + { "temperature", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_raw_pressure_t, temperature) }, \ + } \ +} /** @@ -27,22 +41,35 @@ typedef struct __mavlink_raw_pressure_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(press_abs, i, msg->payload); // Absolute pressure (raw) - i += put_int16_t_by_index(press_diff1, i, msg->payload); // Differential pressure 1 (raw) - i += put_int16_t_by_index(press_diff2, i, msg->payload); // Differential pressure 2 (raw) - i += put_int16_t_by_index(temperature, i, msg->payload); // Raw Temperature measurement (raw) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 16); } /** - * @brief Pack a raw_pressure message + * @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 @@ -54,18 +81,32 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(press_abs, i, msg->payload); // Absolute pressure (raw) - i += put_int16_t_by_index(press_diff1, i, msg->payload); // Differential pressure 1 (raw) - i += put_int16_t_by_index(press_diff2, i, msg->payload); // Differential pressure 2 (raw) - i += put_int16_t_by_index(temperature, i, msg->payload); // Raw Temperature measurement (raw) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); } /** @@ -95,14 +136,32 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ 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) { - mavlink_message_t msg; - mavlink_msg_raw_pressure_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, press_abs, press_diff1, press_diff2, temperature); - mavlink_send_uart(chan, &msg); +#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 * @@ -110,16 +169,7 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_raw_pressure_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -129,10 +179,7 @@ static inline uint64_t mavlink_msg_raw_pressure_get_usec(const mavlink_message_t */ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -142,10 +189,7 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_messa */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -155,10 +199,7 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -168,10 +209,7 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -182,9 +220,13 @@ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_mes */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_override.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_override.h index 7595857a5e..f254ece156 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_override.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_override.h @@ -2,21 +2,40 @@ #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 -typedef struct __mavlink_rc_channels_override_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_rc_channels_override_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_rc_channels_override_t, target_component) }, \ + { "chan1_raw", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ + { "chan2_raw", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_override_t, chan2_raw) }, \ + { "chan3_raw", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_override_t, chan3_raw) }, \ + { "chan4_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_override_t, chan4_raw) }, \ + { "chan5_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan5_raw) }, \ + { "chan6_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ + { "chan7_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ + { "chan8_raw", MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + } \ +} /** @@ -37,27 +56,45 @@ typedef struct __mavlink_rc_channels_override_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds - i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds - i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds - i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds - i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds - i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds - i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds - i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 18); } /** - * @brief Pack a rc_channels_override message + * @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 @@ -74,23 +111,42 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds - i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds - i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds - i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds - i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds - i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds - i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds - i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); } /** @@ -125,14 +181,42 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id 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) { - mavlink_message_t msg; - mavlink_msg_rc_channels_override_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw); - mavlink_send_uart(chan, &msg); +#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 * @@ -140,7 +224,7 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -150,7 +234,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const m */ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -160,10 +244,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -173,10 +254,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -186,10 +264,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -199,10 +274,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -212,10 +284,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -225,10 +294,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -238,10 +304,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -251,10 +314,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -265,6 +325,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavl */ 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); @@ -275,4 +336,7 @@ static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message 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/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_raw.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_raw.h index 0443838bdc..461955b6f3 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_raw.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_raw.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 -typedef struct __mavlink_rc_channels_raw_t +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% - + 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", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_raw_t, chan1_raw) }, \ + { "chan2_raw", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_rc_channels_raw_t, chan2_raw) }, \ + { "chan3_raw", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_rc_channels_raw_t, chan3_raw) }, \ + { "chan4_raw", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_rc_channels_raw_t, chan4_raw) }, \ + { "chan5_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_rc_channels_raw_t, chan5_raw) }, \ + { "chan6_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_raw_t, chan6_raw) }, \ + { "chan7_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_raw_t, chan7_raw) }, \ + { "chan8_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_raw_t, chan8_raw) }, \ + { "rssi", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_raw_t, rssi) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_rc_channels_raw_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 17); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - - i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds - i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds - i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds - i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds - i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds - i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds - i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds - i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 17); } /** - * @brief Pack a rc_channels_raw message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 17); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - - i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds - i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds - i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds - i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds - i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds - i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds - i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds - i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin 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) { - mavlink_message_t msg; - mavlink_msg_rc_channels_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,10 +213,7 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -147,10 +223,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -160,10 +233,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -173,10 +243,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -186,10 +253,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -199,10 +263,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -212,10 +273,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -225,10 +283,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -238,7 +293,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_m */ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -249,6 +304,7 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message */ 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); @@ -258,4 +314,7 @@ static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* m 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/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_scaled.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_scaled.h index 526482b4e0..042d0ae902 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_rc_channels_scaled.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 -typedef struct __mavlink_rc_channels_scaled_t +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% - + 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", MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_rc_channels_scaled_t, chan1_scaled) }, \ + { "chan2_scaled", MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_rc_channels_scaled_t, chan2_scaled) }, \ + { "chan3_scaled", MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_rc_channels_scaled_t, chan3_scaled) }, \ + { "chan4_scaled", MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_rc_channels_scaled_t, chan4_scaled) }, \ + { "chan5_scaled", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_rc_channels_scaled_t, chan5_scaled) }, \ + { "chan6_scaled", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_rc_channels_scaled_t, chan6_scaled) }, \ + { "chan7_scaled", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_rc_channels_scaled_t, chan7_scaled) }, \ + { "chan8_scaled", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_rc_channels_scaled_t, chan8_scaled) }, \ + { "rssi", MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_scaled_t, rssi) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_rc_channels_scaled_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 17); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - - i += put_int16_t_by_index(chan1_scaled, i, msg->payload); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan2_scaled, i, msg->payload); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan3_scaled, i, msg->payload); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan4_scaled, i, msg->payload); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan5_scaled, i, msg->payload); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan6_scaled, i, msg->payload); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan7_scaled, i, msg->payload); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan8_scaled, i, msg->payload); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 17); } /** - * @brief Pack a rc_channels_scaled message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 17); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - - i += put_int16_t_by_index(chan1_scaled, i, msg->payload); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan2_scaled, i, msg->payload); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan3_scaled, i, msg->payload); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan4_scaled, i, msg->payload); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan5_scaled, i, msg->payload); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan6_scaled, i, msg->payload); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan7_scaled, i, msg->payload); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_int16_t_by_index(chan8_scaled, i, msg->payload); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - i += put_uint8_t_by_index(rssi, i, msg->payload); // Receive signal strength indicator, 0: 0%, 255: 100% - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, 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) { - mavlink_message_t msg; - mavlink_msg_rc_channels_scaled_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,10 +213,7 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, i */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 0); } /** @@ -147,10 +223,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 2); } /** @@ -160,10 +233,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 4); } /** @@ -173,10 +243,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 6); } /** @@ -186,10 +253,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -199,10 +263,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -212,10 +273,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -225,10 +283,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -238,7 +293,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavl */ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -249,6 +304,7 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_mess */ 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); @@ -258,4 +314,7 @@ static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t 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/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h index 15ee1a6a5f..5cbd3d5efa 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h @@ -2,16 +2,30 @@ #define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 -typedef struct __mavlink_request_data_stream_t +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. - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_request_data_stream_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_request_data_stream_t, target_component) }, \ + { "req_stream_id", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_request_data_stream_t, req_stream_id) }, \ + { "req_message_rate", MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_request_data_stream_t, req_message_rate) }, \ + { "start_stop", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_request_data_stream_t, start_stop) }, \ + } \ +} /** @@ -27,22 +41,35 @@ typedef struct __mavlink_request_data_stream_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 6); +#endif + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz - i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 6); } /** - * @brief Pack a request_data_stream message + * @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 @@ -54,18 +81,32 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 6); +#endif + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. - i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz - i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6); } /** @@ -95,14 +136,32 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, 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) { - mavlink_message_t msg; - mavlink_msg_request_data_stream_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, req_stream_id, req_message_rate, start_stop); - mavlink_send_uart(chan, &msg); +#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 * @@ -110,7 +169,7 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +179,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const ma */ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -130,7 +189,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const */ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -140,10 +199,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma */ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 3); } /** @@ -153,7 +209,7 @@ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(cons */ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -164,9 +220,13 @@ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavli */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index a561da0667..5ac8ebb087 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -2,16 +2,30 @@ #define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 58 -typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t +typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t { - uint32_t time_ms; ///< Timestamp in milliseconds since system boot - 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 - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, time_us) }, \ + { "roll_speed", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, roll_speed) }, \ + { "pitch_speed", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, pitch_speed) }, \ + { "yaw_speed", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, yaw_speed) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t, thrust) }, \ + } \ +} /** @@ -20,52 +34,79 @@ typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param time_ms Timestamp in milliseconds since system boot + * @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, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; - - i += put_uint32_t_by_index(time_ms, i, msg->payload); // Timestamp in milliseconds since system boot - i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s - i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s - i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 24); } /** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @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_ms Timestamp in milliseconds since system boot + * @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, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; - - i += put_uint32_t_by_index(time_ms, i, msg->payload); // Timestamp in milliseconds since system boot - i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s - i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s - i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24); } /** @@ -78,14 +119,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha */ 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_ms, 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); + 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_ms Timestamp in milliseconds since system boot + * @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 @@ -93,29 +134,42 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +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) { - mavlink_message_t msg; - mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_ms, roll_speed, pitch_speed, yaw_speed, thrust); - mavlink_send_uart(chan, &msg); +#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_ms from roll_pitch_yaw_speed_thrust_setpoint message + * @brief Get field time_us from roll_pitch_yaw_speed_thrust_setpoint message * - * @return Timestamp in milliseconds since system boot + * @return Timestamp in micro seconds since unix epoch */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_ms(const mavlink_message_t* msg) +static inline uint64_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -125,12 +179,7 @@ static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -140,12 +189,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_sp */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -155,12 +199,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_s */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -170,12 +209,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_spe */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -186,9 +220,13 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust( */ 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) { - roll_pitch_yaw_speed_thrust_setpoint->time_ms = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_ms(msg); +#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/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index 96bfc6cd26..5fbe945fa5 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -2,16 +2,30 @@ #define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 57 -typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t +typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t { - uint32_t time_ms; ///< Timestamp in milliseconds since system boot - 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 - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, time_us) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, yaw) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_roll_pitch_yaw_thrust_setpoint_t, thrust) }, \ + } \ +} /** @@ -20,52 +34,79 @@ typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param time_ms Timestamp in milliseconds since system boot + * @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, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - - i += put_uint32_t_by_index(time_ms, i, msg->payload); // Timestamp in milliseconds since system boot - i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians - i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 24); } /** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @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_ms Timestamp in milliseconds since system boot + * @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, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - - i += put_uint32_t_by_index(time_ms, i, msg->payload); // Timestamp in milliseconds since system boot - i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians - i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24); } /** @@ -78,14 +119,14 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint */ 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_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); + 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_ms Timestamp in milliseconds since system boot + * @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 @@ -93,29 +134,42 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +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) { - mavlink_message_t msg; - mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_ms, roll, pitch, yaw, thrust); - mavlink_send_uart(chan, &msg); +#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_ms from roll_pitch_yaw_thrust_setpoint message + * @brief Get field time_us from roll_pitch_yaw_thrust_setpoint message * - * @return Timestamp in milliseconds since system boot + * @return Timestamp in micro seconds since unix epoch */ -static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(const mavlink_message_t* msg) +static inline uint64_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -125,12 +179,7 @@ static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(co */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -140,12 +189,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const ma */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -155,12 +199,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const m */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -170,12 +209,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mav */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -186,9 +220,13 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const */ 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) { - roll_pitch_yaw_thrust_setpoint->time_ms = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(msg); +#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/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h index e1f9cc6b12..462f82f3e4 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_allowed_area.h @@ -2,18 +2,34 @@ #define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 -typedef struct __mavlink_safety_allowed_area_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_safety_allowed_area_t, frame) }, \ + { "p1x", MAVLINK_TYPE_FLOAT, 0, 1, offsetof(mavlink_safety_allowed_area_t, p1x) }, \ + { "p1y", MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_safety_allowed_area_t, p1y) }, \ + { "p1z", MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_safety_allowed_area_t, p1z) }, \ + { "p2x", MAVLINK_TYPE_FLOAT, 0, 13, offsetof(mavlink_safety_allowed_area_t, p2x) }, \ + { "p2y", MAVLINK_TYPE_FLOAT, 0, 17, offsetof(mavlink_safety_allowed_area_t, p2y) }, \ + { "p2z", MAVLINK_TYPE_FLOAT, 0, 21, offsetof(mavlink_safety_allowed_area_t, p2z) }, \ + } \ +} /** @@ -31,24 +47,39 @@ typedef struct __mavlink_safety_allowed_area_t * @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) +static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; +#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(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(msg), &packet, 25); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 25); } /** - * @brief Pack a safety_allowed_area message + * @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 @@ -62,20 +93,36 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u * @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) +static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) { - uint16_t i = 0; +#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(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(msg), &packet, 25); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25); } /** @@ -107,14 +154,36 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - mavlink_message_t msg; - mavlink_msg_safety_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); +#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 * @@ -122,7 +191,7 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -132,12 +201,7 @@ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_me */ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 1); } /** @@ -147,12 +211,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 5); } /** @@ -162,12 +221,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 9); } /** @@ -177,12 +231,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 13); } /** @@ -192,12 +241,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 17); } /** @@ -207,12 +251,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 21); } /** @@ -223,6 +262,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_messag */ 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); @@ -230,4 +270,7 @@ static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h index da571e8e72..b8b0bc66c8 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_safety_set_allowed_area.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 -typedef struct __mavlink_safety_set_allowed_area_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_safety_set_allowed_area_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_safety_set_allowed_area_t, target_component) }, \ + { "frame", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_safety_set_allowed_area_t, frame) }, \ + { "p1x", MAVLINK_TYPE_FLOAT, 0, 3, offsetof(mavlink_safety_set_allowed_area_t, p1x) }, \ + { "p1y", MAVLINK_TYPE_FLOAT, 0, 7, offsetof(mavlink_safety_set_allowed_area_t, p1y) }, \ + { "p1z", MAVLINK_TYPE_FLOAT, 0, 11, offsetof(mavlink_safety_set_allowed_area_t, p1z) }, \ + { "p2x", MAVLINK_TYPE_FLOAT, 0, 15, offsetof(mavlink_safety_set_allowed_area_t, p2x) }, \ + { "p2y", MAVLINK_TYPE_FLOAT, 0, 19, offsetof(mavlink_safety_set_allowed_area_t, p2y) }, \ + { "p2z", MAVLINK_TYPE_FLOAT, 0, 23, offsetof(mavlink_safety_set_allowed_area_t, p2z) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_safety_set_allowed_area_t * @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) +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - uint16_t i = 0; +#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(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(msg), &packet, 27); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 27); } /** - * @brief Pack a safety_set_allowed_area message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i * @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) +static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) { - uint16_t i = 0; +#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(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(msg), &packet, 27); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 27); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - mavlink_message_t msg; - mavlink_msg_safety_set_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,7 +213,7 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -144,7 +223,7 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(cons */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -154,7 +233,7 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(c */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -164,12 +243,7 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlin */ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 3); } /** @@ -179,12 +253,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 7); } /** @@ -194,12 +263,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 11); } /** @@ -209,12 +273,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 15); } /** @@ -224,12 +283,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 19); } /** @@ -239,12 +293,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 23); } /** @@ -255,6 +304,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_me */ 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); @@ -264,4 +314,7 @@ static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_mess 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/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_imu.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_imu.h index 77637286ff..553bbc5383 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_imu.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_imu.h @@ -2,21 +2,40 @@ #define MAVLINK_MSG_ID_SCALED_IMU 26 -typedef struct __mavlink_scaled_imu_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_scaled_imu_t, usec) }, \ + { "xacc", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_scaled_imu_t, xacc) }, \ + { "yacc", MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_scaled_imu_t, yacc) }, \ + { "zacc", MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_imu_t, zacc) }, \ + { "xgyro", MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_imu_t, xgyro) }, \ + { "ygyro", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, ygyro) }, \ + { "zgyro", MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, zgyro) }, \ + { "xmag", MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, xmag) }, \ + { "ymag", MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, ymag) }, \ + { "zmag", MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_scaled_imu_t, zmag) }, \ + } \ +} /** @@ -37,27 +56,45 @@ typedef struct __mavlink_scaled_imu_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (millirad /sec) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (millirad /sec) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (millirad /sec) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (milli tesla) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (milli tesla) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (milli tesla) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 26); } /** - * @brief Pack a scaled_imu message + * @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 @@ -74,23 +111,42 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg) - i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg) - i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg) - i += put_int16_t_by_index(xgyro, i, msg->payload); // Angular speed around X axis (millirad /sec) - i += put_int16_t_by_index(ygyro, i, msg->payload); // Angular speed around Y axis (millirad /sec) - i += put_int16_t_by_index(zgyro, i, msg->payload); // Angular speed around Z axis (millirad /sec) - i += put_int16_t_by_index(xmag, i, msg->payload); // X Magnetic field (milli tesla) - i += put_int16_t_by_index(ymag, i, msg->payload); // Y Magnetic field (milli tesla) - i += put_int16_t_by_index(zmag, i, msg->payload); // Z Magnetic field (milli tesla) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26); } /** @@ -125,14 +181,42 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t 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) { - mavlink_message_t msg; - mavlink_msg_scaled_imu_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); - mavlink_send_uart(chan, &msg); +#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 * @@ -140,16 +224,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_scaled_imu_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -159,10 +234,7 @@ static inline uint64_t mavlink_msg_scaled_imu_get_usec(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -172,10 +244,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -185,10 +254,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -198,10 +264,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -211,10 +274,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -224,10 +284,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 18); } /** @@ -237,10 +294,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 20); } /** @@ -250,10 +304,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 22); } /** @@ -263,10 +314,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 24); } /** @@ -277,6 +325,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* m */ 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); @@ -287,4 +336,7 @@ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, m 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/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_pressure.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_pressure.h index 08a9bedd37..273228ddc4 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_pressure.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_scaled_pressure.h @@ -2,15 +2,28 @@ #define MAVLINK_MSG_ID_SCALED_PRESSURE 38 -typedef struct __mavlink_scaled_pressure_t +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) - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_scaled_pressure_t, usec) }, \ + { "press_abs", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ + { "press_diff", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ + { "temperature", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + } \ +} /** @@ -25,21 +38,33 @@ typedef struct __mavlink_scaled_pressure_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(press_abs, i, msg->payload); // Absolute pressure (hectopascal) - i += put_float_by_index(press_diff, i, msg->payload); // Differential pressure 1 (hectopascal) - i += put_int16_t_by_index(temperature, i, msg->payload); // Temperature measurement (0.01 degrees celsius) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 18); } /** - * @brief Pack a scaled_pressure message + * @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 @@ -50,17 +75,30 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - i += put_float_by_index(press_abs, i, msg->payload); // Absolute pressure (hectopascal) - i += put_float_by_index(press_diff, i, msg->payload); // Differential pressure 1 (hectopascal) - i += put_int16_t_by_index(temperature, i, msg->payload); // Temperature measurement (0.01 degrees celsius) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); } /** @@ -89,14 +127,30 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { - mavlink_message_t msg; - mavlink_msg_scaled_pressure_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, press_abs, press_diff, temperature); - mavlink_send_uart(chan, &msg); +#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 * @@ -104,16 +158,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_scaled_pressure_get_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -123,12 +168,7 @@ static inline uint64_t mavlink_msg_scaled_pressure_get_usec(const mavlink_messag */ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -138,12 +178,7 @@ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_mess */ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -153,10 +188,7 @@ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_mes */ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -167,8 +199,12 @@ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_ */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h index 751afcb80a..848b82b85d 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_servo_output_raw.h @@ -2,19 +2,36 @@ #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 -typedef struct __mavlink_servo_output_raw_t +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 - + 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", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ + { "servo2_raw", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_servo_output_raw_t, servo2_raw) }, \ + { "servo3_raw", MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo3_raw) }, \ + { "servo4_raw", MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_servo_output_raw_t, servo4_raw) }, \ + { "servo5_raw", MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_servo_output_raw_t, servo5_raw) }, \ + { "servo6_raw", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ + { "servo7_raw", MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ + { "servo8_raw", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + } \ +} /** @@ -33,25 +50,41 @@ typedef struct __mavlink_servo_output_raw_t * @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) +static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - uint16_t i = 0; +#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(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(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 16); } /** - * @brief Pack a servo_output_raw message + * @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 @@ -66,21 +99,38 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @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) +static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) { - uint16_t i = 0; +#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(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(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16); } /** @@ -113,14 +163,38 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { - mavlink_message_t msg; - mavlink_msg_servo_output_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); - mavlink_send_uart(chan, &msg); +#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 * @@ -128,10 +202,7 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -141,10 +212,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -154,10 +222,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -167,10 +232,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -180,10 +242,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -193,10 +252,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -206,10 +262,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -219,10 +272,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -233,6 +283,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink */ 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); @@ -241,4 +292,7 @@ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* 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/libraries/GCS_MAVLink/include/common/mavlink_msg_set_altitude.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_altitude.h index 775db90e34..c49941bac4 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_altitude.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_altitude.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_SET_ALTITUDE 65 -typedef struct __mavlink_set_altitude_t +typedef struct __mavlink_set_altitude_t { - uint8_t target; ///< The system setting the altitude - uint32_t mode; ///< The new altitude in meters - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_altitude_t, target) }, \ + { "mode", MAVLINK_TYPE_UINT32_T, 0, 1, offsetof(mavlink_set_altitude_t, mode) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_set_altitude_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 5); +#else + mavlink_set_altitude_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 5); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); // The new altitude in meters - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 5); } /** - * @brief Pack a set_altitude message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 5); +#else + mavlink_set_altitude_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 5); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the altitude - i += put_uint32_t_by_index(mode, i, msg->payload); // The new altitude in meters - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) { - mavlink_message_t msg; - mavlink_msg_set_altitude_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, mode); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,12 +146,7 @@ static inline uint8_t mavlink_msg_set_altitude_get_target(const mavlink_message_ */ static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint32_t(msg, 1); } /** @@ -118,6 +157,10 @@ static inline uint32_t mavlink_msg_set_altitude_get_mode(const mavlink_message_t */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_set_mode.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_mode.h index efeea6509f..9836756d19 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_mode.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_mode.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_SET_MODE 11 -typedef struct __mavlink_set_mode_t +typedef struct __mavlink_set_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t mode; ///< The new mode - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_mode_t, target) }, \ + { "mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_mode_t, mode) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_set_mode_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 2); +#else + mavlink_set_mode_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MODE; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(mode, i, msg->payload); // The new mode - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a set_mode message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 2); +#else + mavlink_set_mode_t packet; + packet.target = target; + packet.mode = mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MODE; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(mode, i, msg->payload); // The new mode - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { - mavlink_message_t msg; - mavlink_msg_set_mode_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, mode); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,7 +146,7 @@ static inline uint8_t mavlink_msg_set_mode_get_target(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -113,6 +157,10 @@ static inline uint8_t mavlink_msg_set_mode_get_mode(const mavlink_message_t* msg */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_set_nav_mode.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_nav_mode.h index fe514b2ea5..c808b453d0 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_nav_mode.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_nav_mode.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_SET_NAV_MODE 12 -typedef struct __mavlink_set_nav_mode_t +typedef struct __mavlink_set_nav_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t nav_mode; ///< The new navigation mode - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_nav_mode_t, target) }, \ + { "nav_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_nav_mode_t, nav_mode) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_set_nav_mode_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, nav_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_set_nav_mode_t packet; + packet.target = target; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // The new navigation mode - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a set_nav_mode message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, nav_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_set_nav_mode_t packet; + packet.target = target; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the mode - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // The new navigation mode - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) { - mavlink_message_t msg; - mavlink_msg_set_nav_mode_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, nav_mode); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,7 +146,7 @@ static inline uint8_t mavlink_msg_set_nav_mode_get_target(const mavlink_message_ */ static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -113,6 +157,10 @@ static inline uint8_t mavlink_msg_set_nav_mode_get_nav_mode(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index dd12ea4cb0..e8f2399750 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -2,17 +2,32 @@ #define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 56 -typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, target_component) }, \ + { "roll_speed", MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, roll_speed) }, \ + { "pitch_speed", MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, pitch_speed) }, \ + { "yaw_speed", MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, yaw_speed) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_set_roll_pitch_yaw_speed_thrust_t, thrust) }, \ + } \ +} /** @@ -29,23 +44,37 @@ typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s - i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s - i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 18); } /** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @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 @@ -58,19 +87,34 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s - i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s - i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); } /** @@ -101,14 +145,34 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ 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) { - mavlink_message_t msg; - mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust); - mavlink_send_uart(chan, &msg); +#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 * @@ -116,7 +180,7 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -126,7 +190,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_sys */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -136,12 +200,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_com */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 2); } /** @@ -151,12 +210,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(c */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 6); } /** @@ -166,12 +220,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed( */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 10); } /** @@ -181,12 +230,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(co */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 14); } /** @@ -197,10 +241,14 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 7bf7d26247..9ad4c32562 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -2,17 +2,32 @@ #define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 55 -typedef struct __mavlink_set_roll_pitch_yaw_thrust_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, target_component) }, \ + { "roll", MAVLINK_TYPE_FLOAT, 0, 2, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, roll) }, \ + { "pitch", MAVLINK_TYPE_FLOAT, 0, 6, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, pitch) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 10, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, yaw) }, \ + { "thrust", MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_set_roll_pitch_yaw_thrust_t, thrust) }, \ + } \ +} /** @@ -29,23 +44,37 @@ typedef struct __mavlink_set_roll_pitch_yaw_thrust_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians - i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 18); } /** - * @brief Pack a set_roll_pitch_yaw_thrust message + * @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 @@ -58,19 +87,34 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians - i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians - i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians - i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1 - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18); } /** @@ -101,14 +145,34 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst 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) { - mavlink_message_t msg; - mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw, thrust); - mavlink_send_uart(chan, &msg); +#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 * @@ -116,7 +180,7 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -126,7 +190,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(co */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -136,12 +200,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 2); } /** @@ -151,12 +210,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 6); } /** @@ -166,12 +220,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlin */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 10); } /** @@ -181,12 +230,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_ */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 14); } /** @@ -197,10 +241,14 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavli */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_state_correction.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_state_correction.h index c4c6b5abc9..a06fac2444 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_state_correction.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_state_correction.h @@ -2,20 +2,38 @@ #define MAVLINK_MSG_ID_STATE_CORRECTION 64 -typedef struct __mavlink_state_correction_t +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 - + 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", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_state_correction_t, xErr) }, \ + { "yErr", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_state_correction_t, yErr) }, \ + { "zErr", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_state_correction_t, zErr) }, \ + { "rollErr", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_state_correction_t, rollErr) }, \ + { "pitchErr", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_state_correction_t, pitchErr) }, \ + { "yawErr", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_state_correction_t, yawErr) }, \ + { "vxErr", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_state_correction_t, vxErr) }, \ + { "vyErr", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_state_correction_t, vyErr) }, \ + { "vzErr", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_state_correction_t, vzErr) }, \ + } \ +} /** @@ -35,26 +53,43 @@ typedef struct __mavlink_state_correction_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - - i += put_float_by_index(xErr, i, msg->payload); // x position error - i += put_float_by_index(yErr, i, msg->payload); // y position error - i += put_float_by_index(zErr, i, msg->payload); // z position error - i += put_float_by_index(rollErr, i, msg->payload); // roll error (radians) - i += put_float_by_index(pitchErr, i, msg->payload); // pitch error (radians) - i += put_float_by_index(yawErr, i, msg->payload); // yaw error (radians) - i += put_float_by_index(vxErr, i, msg->payload); // x velocity - i += put_float_by_index(vyErr, i, msg->payload); // y velocity - i += put_float_by_index(vzErr, i, msg->payload); // z velocity - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 36); } /** - * @brief Pack a state_correction message + * @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 @@ -70,22 +105,40 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - - i += put_float_by_index(xErr, i, msg->payload); // x position error - i += put_float_by_index(yErr, i, msg->payload); // y position error - i += put_float_by_index(zErr, i, msg->payload); // z position error - i += put_float_by_index(rollErr, i, msg->payload); // roll error (radians) - i += put_float_by_index(pitchErr, i, msg->payload); // pitch error (radians) - i += put_float_by_index(yawErr, i, msg->payload); // yaw error (radians) - i += put_float_by_index(vxErr, i, msg->payload); // x velocity - i += put_float_by_index(vyErr, i, msg->payload); // y velocity - i += put_float_by_index(vzErr, i, msg->payload); // z velocity - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36); } /** @@ -119,14 +172,40 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui 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) { - mavlink_message_t msg; - mavlink_msg_state_correction_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr); - mavlink_send_uart(chan, &msg); +#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 * @@ -134,12 +213,7 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 0); } /** @@ -149,12 +223,7 @@ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -164,12 +233,7 @@ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -179,12 +243,7 @@ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -194,12 +253,7 @@ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_messa */ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -209,12 +263,7 @@ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_mess */ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -224,12 +273,7 @@ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_messag */ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 24); } /** @@ -239,12 +283,7 @@ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 28); } /** @@ -254,12 +293,7 @@ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 32); } /** @@ -270,6 +304,7 @@ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message */ 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); @@ -279,4 +314,7 @@ static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* 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/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h index 681b659c4d..d39f9b850a 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_statustext.h @@ -2,15 +2,25 @@ #define MAVLINK_MSG_ID_STATUSTEXT 254 -typedef struct __mavlink_statustext_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ + { "text", MAVLINK_TYPE_INT8_T, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + } \ +} + /** * @brief Pack a statustext message @@ -22,19 +32,27 @@ typedef struct __mavlink_statustext_t * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + memcpy(packet.text, text, sizeof(int8_t)*50); + memcpy(_MAV_PAYLOAD(msg), &packet, 51); +#endif + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - - i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 51); } /** - * @brief Pack a statustext message + * @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 @@ -43,15 +61,24 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @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) +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) { - uint16_t i = 0; +#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(msg), buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + memcpy(packet.text, text, sizeof(int8_t)*50); + memcpy(_MAV_PAYLOAD(msg), &packet, 51); +#endif + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - - i += put_uint8_t_by_index(severity, i, msg->payload); // Severity of status, 0 = info message, 255 = critical fault - i += put_array_by_index((const int8_t*)text, sizeof(int8_t)*50, i, msg->payload); // Status text message, without null termination character - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51); } /** @@ -76,16 +103,26 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text) +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t *text) { - mavlink_message_t msg; - mavlink_msg_statustext_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, severity, text); - mavlink_send_uart(chan, &msg); +#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; + 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 * @@ -93,7 +130,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -101,11 +138,9 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_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* r_data) +static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t *text) { - - memcpy(r_data, msg->payload+sizeof(uint8_t), sizeof(int8_t)*50); - return sizeof(int8_t)*50; + return _MAV_RETURN_int8_t_array(msg, text, 50, 1); } /** @@ -116,6 +151,10 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h index ef83d84481..1113149fa4 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_sys_status.h @@ -2,18 +2,34 @@ #define MAVLINK_MSG_ID_SYS_STATUS 34 -typedef struct __mavlink_sys_status_t +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) - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_sys_status_t, mode) }, \ + { "nav_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_sys_status_t, nav_mode) }, \ + { "status", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_sys_status_t, status) }, \ + { "load", MAVLINK_TYPE_UINT16_T, 0, 3, offsetof(mavlink_sys_status_t, load) }, \ + { "vbat", MAVLINK_TYPE_UINT16_T, 0, 5, offsetof(mavlink_sys_status_t, vbat) }, \ + { "battery_remaining", MAVLINK_TYPE_UINT16_T, 0, 7, offsetof(mavlink_sys_status_t, battery_remaining) }, \ + { "packet_drop", MAVLINK_TYPE_UINT16_T, 0, 9, offsetof(mavlink_sys_status_t, packet_drop) }, \ + } \ +} /** @@ -31,24 +47,39 @@ typedef struct __mavlink_sys_status_t * @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) +static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - uint16_t i = 0; +#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(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(msg), &packet, 11); +#endif + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM - i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) - i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 11); } /** - * @brief Pack a sys_status message + * @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 @@ -62,20 +93,36 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @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) +static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t mode,uint8_t nav_mode,uint8_t status,uint16_t load,uint16_t vbat,uint16_t battery_remaining,uint16_t packet_drop) { - uint16_t i = 0; +#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(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(msg), &packet, 11); +#endif + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see MAV_NAV_MODE ENUM - i += put_uint8_t_by_index(status, i, msg->payload); // System status flag, see MAV_STATUS ENUM - i += put_uint16_t_by_index(load, i, msg->payload); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage, in millivolts (1 = 1 millivolt) - i += put_uint16_t_by_index(battery_remaining, i, msg->payload); // Remaining battery energy: (0%: 0, 100%: 1000) - i += put_uint16_t_by_index(packet_drop, i, msg->payload); // Dropped packets (packets that were corrupted on reception on the MAV) - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11); } /** @@ -107,14 +154,36 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { - mavlink_message_t msg; - mavlink_msg_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop); - mavlink_send_uart(chan, &msg); +#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 * @@ -122,7 +191,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t m */ static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -132,7 +201,7 @@ static inline uint8_t mavlink_msg_sys_status_get_mode(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -142,7 +211,7 @@ static inline uint8_t mavlink_msg_sys_status_get_nav_mode(const mavlink_message_ */ static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -152,10 +221,7 @@ static inline uint8_t mavlink_msg_sys_status_get_status(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 3); } /** @@ -165,10 +231,7 @@ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 5); } /** @@ -178,10 +241,7 @@ static inline uint16_t mavlink_msg_sys_status_get_vbat(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 7); } /** @@ -191,10 +251,7 @@ static inline uint16_t mavlink_msg_sys_status_get_battery_remaining(const mavlin */ static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 9); } /** @@ -205,6 +262,7 @@ static inline uint16_t mavlink_msg_sys_status_get_packet_drop(const mavlink_mess */ 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); @@ -212,4 +270,7 @@ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, m 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/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time.h index d293f025bf..4473dff1cb 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time.h @@ -2,12 +2,22 @@ #define MAVLINK_MSG_ID_SYSTEM_TIME 2 -typedef struct __mavlink_system_time_t +typedef struct __mavlink_system_time_t { - uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. - + 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", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_system_time_t, time_usec) }, \ + } \ +} /** @@ -19,18 +29,27 @@ typedef struct __mavlink_system_time_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint64_t(buf, 0, time_usec); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_system_time_t packet; + packet.time_usec = time_usec; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - - i += put_uint64_t_by_index(time_usec, i, msg->payload); // Timestamp of the master clock in microseconds since UNIX epoch. - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 8); } /** - * @brief Pack a system_time message + * @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 @@ -38,14 +57,24 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint64_t(buf, 0, time_usec); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_system_time_t packet; + packet.time_usec = time_usec; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - - i += put_uint64_t_by_index(time_usec, i, msg->payload); // Timestamp of the master clock in microseconds since UNIX epoch. - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); } /** @@ -71,14 +100,24 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { - mavlink_message_t msg; - mavlink_msg_system_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_usec); - mavlink_send_uart(chan, &msg); +#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 * @@ -86,16 +125,7 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload)[0]; - r.b[6] = (msg->payload)[1]; - r.b[5] = (msg->payload)[2]; - r.b[4] = (msg->payload)[3]; - r.b[3] = (msg->payload)[4]; - r.b[2] = (msg->payload)[5]; - r.b[1] = (msg->payload)[6]; - r.b[0] = (msg->payload)[7]; - return (uint64_t)r.ll; + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -106,5 +136,9 @@ static inline uint64_t mavlink_msg_system_time_get_time_usec(const mavlink_messa */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h index 75c25f1ced..39a45237da 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_system_time_utc.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 -typedef struct __mavlink_system_time_utc_t +typedef struct __mavlink_system_time_utc_t { - uint32_t utc_date; ///< GPS UTC date ddmmyy - uint32_t utc_time; ///< GPS UTC time hhmmss - + 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", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_system_time_utc_t, utc_date) }, \ + { "utc_time", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_system_time_utc_t, utc_time) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_system_time_utc_t * @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) +static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t utc_date, uint32_t utc_time) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, utc_date); + _mav_put_uint32_t(buf, 4, utc_time); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 8); } /** - * @brief Pack a system_time_utc message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 * @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) +static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t utc_date,uint32_t utc_time) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, utc_date); + _mav_put_uint32_t(buf, 4, utc_time); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { - mavlink_message_t msg; - mavlink_msg_system_time_utc_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, utc_date, utc_time); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,12 +136,7 @@ static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint */ static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -107,12 +146,7 @@ static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_me */ static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + return _MAV_RETURN_uint32_t(msg, 4); } /** @@ -123,6 +157,10 @@ static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_me */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h index 9f55bdfa70..064d3d0362 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_vfr_hud.h @@ -2,17 +2,32 @@ #define MAVLINK_MSG_ID_VFR_HUD 74 -typedef struct __mavlink_vfr_hud_t +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 - + 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", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_vfr_hud_t, airspeed) }, \ + { "groundspeed", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_vfr_hud_t, groundspeed) }, \ + { "heading", MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_vfr_hud_t, heading) }, \ + { "throttle", MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_vfr_hud_t, throttle) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vfr_hud_t, alt) }, \ + { "climb", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vfr_hud_t, climb) }, \ + } \ +} /** @@ -29,23 +44,37 @@ typedef struct __mavlink_vfr_hud_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, in compass units (0..360, 0=north) - i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 - i += put_float_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters - i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 20); } /** - * @brief Pack a vfr_hud message + * @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 @@ -58,19 +87,34 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, in compass units (0..360, 0=north) - i += put_uint16_t_by_index(throttle, i, msg->payload); // Current throttle setting in integer percent, 0 to 100 - i += put_float_by_index(alt, i, msg->payload); // Current altitude (MSL), in meters - i += put_float_by_index(climb, i, msg->payload); // Current climb rate in meters/second - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20); } /** @@ -101,14 +145,34 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com 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) { - mavlink_message_t msg; - mavlink_msg_vfr_hud_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, airspeed, groundspeed, heading, throttle, alt, climb); - mavlink_send_uart(chan, &msg); +#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 * @@ -116,12 +180,7 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe */ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload)[0]; - r.b[2] = (msg->payload)[1]; - r.b[1] = (msg->payload)[2]; - r.b[0] = (msg->payload)[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 0); } /** @@ -131,12 +190,7 @@ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* ms */ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 4); } /** @@ -146,10 +200,7 @@ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* */ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -159,10 +210,7 @@ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* m */ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -172,12 +220,7 @@ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* */ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -187,12 +230,7 @@ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -203,10 +241,14 @@ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h index ab1c5f836a..bce7018b25 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint.h @@ -2,25 +2,48 @@ #define MAVLINK_MSG_ID_WAYPOINT 39 -typedef struct __mavlink_waypoint_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_t, target_component) }, \ + { "seq", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_t, seq) }, \ + { "frame", MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_waypoint_t, frame) }, \ + { "command", MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_waypoint_t, command) }, \ + { "current", MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_waypoint_t, current) }, \ + { "autocontinue", MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_waypoint_t, autocontinue) }, \ + { "param1", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_waypoint_t, param1) }, \ + { "param2", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_waypoint_t, param2) }, \ + { "param3", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_waypoint_t, param3) }, \ + { "param4", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_waypoint_t, param4) }, \ + { "x", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_waypoint_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_waypoint_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_waypoint_t, z) }, \ + } \ +} /** @@ -45,31 +68,53 @@ typedef struct __mavlink_waypoint_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - i += put_uint8_t_by_index(frame, i, msg->payload); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - i += put_uint8_t_by_index(command, i, msg->payload); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - i += put_uint8_t_by_index(current, i, msg->payload); // false:0, true:1 - i += put_uint8_t_by_index(autocontinue, i, msg->payload); // autocontinue to next wp - i += put_float_by_index(param1, i, msg->payload); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - i += put_float_by_index(param3, i, msg->payload); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude - i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 36); } /** - * @brief Pack a waypoint message + * @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 @@ -90,27 +135,50 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - i += put_uint8_t_by_index(frame, i, msg->payload); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - i += put_uint8_t_by_index(command, i, msg->payload); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - i += put_uint8_t_by_index(current, i, msg->payload); // false:0, true:1 - i += put_uint8_t_by_index(autocontinue, i, msg->payload); // autocontinue to next wp - i += put_float_by_index(param1, i, msg->payload); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - i += put_float_by_index(param2, i, msg->payload); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - i += put_float_by_index(param3, i, msg->payload); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - i += put_float_by_index(param4, i, msg->payload); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - i += put_float_by_index(x, i, msg->payload); // PARAM5 / local: x position, global: latitude - i += put_float_by_index(y, i, msg->payload); // PARAM6 / y position: global: longitude - i += put_float_by_index(z, i, msg->payload); // PARAM7 / z position: global: altitude - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36); } /** @@ -149,14 +217,50 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co 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) { - mavlink_message_t msg; - mavlink_msg_waypoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z); - mavlink_send_uart(chan, &msg); +#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 * @@ -164,7 +268,7 @@ static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -174,7 +278,7 @@ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_messa */ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -184,10 +288,7 @@ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_me */ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -197,7 +298,7 @@ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -207,7 +308,7 @@ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -217,7 +318,7 @@ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -227,7 +328,7 @@ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 7); } /** @@ -237,12 +338,7 @@ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_messag */ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 8); } /** @@ -252,12 +348,7 @@ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 12); } /** @@ -267,12 +358,7 @@ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 16); } /** @@ -282,12 +368,7 @@ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 20); } /** @@ -297,12 +378,7 @@ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 24); } /** @@ -312,12 +388,7 @@ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 28); } /** @@ -327,12 +398,7 @@ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + return _MAV_RETURN_float(msg, 32); } /** @@ -343,6 +409,7 @@ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) */ 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); @@ -357,4 +424,7 @@ static inline void mavlink_msg_waypoint_decode(const mavlink_message_t* msg, mav 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_ack.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_ack.h index d994eaf49a..271d3d3c44 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_ack.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_ack.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_WAYPOINT_ACK 47 -typedef struct __mavlink_waypoint_ack_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_ack_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_ack_t, target_component) }, \ + { "type", MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_waypoint_ack_t, type) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_waypoint_ack_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_waypoint_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(type, i, msg->payload); // 0: OK, 1: Error - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 3); } /** - * @brief Pack a waypoint_ack message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_waypoint_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(type, i, msg->payload); // 0: OK, 1: Error - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { - mavlink_message_t msg; - mavlink_msg_waypoint_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, type); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,7 +147,7 @@ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -108,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -118,7 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlin */ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -129,7 +178,11 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_clear_all.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_clear_all.h index 5020fbada8..a25f608bed 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_clear_all.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 -typedef struct __mavlink_waypoint_clear_all_t +typedef struct __mavlink_waypoint_clear_all_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_clear_all_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_clear_all_t, target_component) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_waypoint_clear_all_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a waypoint_clear_all message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_message_t msg; - mavlink_msg_waypoint_clear_all_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,7 +146,7 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mav */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -113,6 +157,10 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_count.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_count.h index 9ceacb0806..7fb6bfdcea 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_count.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_count.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 -typedef struct __mavlink_waypoint_count_t +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 - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_count_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_count_t, target_component) }, \ + { "count", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_count_t, count) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_waypoint_count_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(count, i, msg->payload); // Number of Waypoints in the Sequence - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 4); } /** - * @brief Pack a waypoint_count message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(count, i, msg->payload); // Number of Waypoints in the Sequence - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { - mavlink_message_t msg; - mavlink_msg_waypoint_count_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, count); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,7 +147,7 @@ static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -108,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink */ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -118,10 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavl */ static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -132,7 +178,11 @@ static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_current.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_current.h index 17f66fc298..086a5f3462 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_current.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_current.h @@ -2,12 +2,22 @@ #define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 -typedef struct __mavlink_waypoint_current_t +typedef struct __mavlink_waypoint_current_t { - uint16_t seq; ///< Sequence - + 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", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_current_t, seq) }, \ + } \ +} /** @@ -19,18 +29,27 @@ typedef struct __mavlink_waypoint_current_t * @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) +static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seq) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a waypoint_current message + * @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 @@ -38,14 +57,24 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -71,14 +100,24 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_current_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq); - mavlink_send_uart(chan, &msg); +#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 * @@ -86,10 +125,7 @@ static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -100,5 +136,9 @@ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_reached.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_reached.h index ffcea52fc9..515d32c12f 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_reached.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_reached.h @@ -2,12 +2,22 @@ #define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 -typedef struct __mavlink_waypoint_reached_t +typedef struct __mavlink_waypoint_reached_t { - uint16_t seq; ///< Sequence - + 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", MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_waypoint_reached_t, seq) }, \ + } \ +} /** @@ -19,18 +29,27 @@ typedef struct __mavlink_waypoint_reached_t * @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) +static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t seq) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a waypoint_reached message + * @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 @@ -38,14 +57,24 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -71,14 +100,24 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_reached_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seq); - mavlink_send_uart(chan, &msg); +#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 * @@ -86,10 +125,7 @@ static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload)[0]; - r.b[0] = (msg->payload)[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -100,5 +136,9 @@ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request.h index 234b55c11d..9f16849f63 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 -typedef struct __mavlink_waypoint_request_t +typedef struct __mavlink_waypoint_request_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_request_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_request_t, target_component) }, \ + { "seq", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_request_t, seq) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_waypoint_request_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 4); } /** - * @brief Pack a waypoint_request message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_request_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,7 +147,7 @@ static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -108,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavli */ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -118,10 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const ma */ static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -132,7 +178,11 @@ static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_messag */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request_list.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request_list.h index d4f7cf03d5..8ab2a578f1 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request_list.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_request_list.h @@ -2,13 +2,24 @@ #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 -typedef struct __mavlink_waypoint_request_list_t +typedef struct __mavlink_waypoint_request_list_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_request_list_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_request_list_t, target_component) }, \ + } \ +} /** @@ -21,19 +32,29 @@ typedef struct __mavlink_waypoint_request_list_t * @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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 2); } /** - * @brief Pack a waypoint_request_list message + * @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 @@ -42,15 +63,26 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t 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) +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) { - uint16_t i = 0; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2); } /** @@ -77,14 +109,26 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - mavlink_message_t msg; - mavlink_msg_waypoint_request_list_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component); - mavlink_send_uart(chan, &msg); +#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 * @@ -92,7 +136,7 @@ static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -102,7 +146,7 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -113,6 +157,10 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(con */ 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/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_current.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_current.h index 6570fe4f82..b75b6863d6 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_current.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_waypoint_set_current.h @@ -2,14 +2,26 @@ #define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 -typedef struct __mavlink_waypoint_set_current_t +typedef struct __mavlink_waypoint_set_current_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence - + 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", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_waypoint_set_current_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_waypoint_set_current_t, target_component) }, \ + { "seq", MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_waypoint_set_current_t, seq) }, \ + } \ +} /** @@ -23,20 +35,31 @@ typedef struct __mavlink_waypoint_set_current_t * @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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, 4); } /** - * @brief Pack a waypoint_set_current message + * @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 @@ -46,16 +69,28 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_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) +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) { - uint16_t i = 0; +#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(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(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4); } /** @@ -83,14 +118,28 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - mavlink_message_t msg; - mavlink_msg_waypoint_set_current_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, seq); - mavlink_send_uart(chan, &msg); +#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 * @@ -98,7 +147,7 @@ static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -108,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const m */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -118,10 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(cons */ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -132,7 +178,11 @@ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_me */ 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/libraries/GCS_MAVLink/include/common/testsuite.h b/libraries/GCS_MAVLink/include/common/testsuite.h new file mode 100644 index 0000000000..2565fb8245 --- /dev/null +++ b/libraries/GCS_MAVLink/include/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; imagic = MAVLINK_STX; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; + checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + + return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + */ +#if MAVLINK_CRC_EXTRA +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint16_t length, uint8_t crc_extra) +{ + return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra); +} +#else +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint16_t length) +{ + return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); +} +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); + +/** + * @brief Finalize a MAVLink message with channel assignment and send + */ +#if MAVLINK_CRC_EXTRA +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, + uint8_t length, uint8_t crc_extra) +#else +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) +#endif +{ + uint16_t checksum; + uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; + uint8_t ck[2]; + mavlink_status_t *status = mavlink_get_channel_status(chan); + buf[0] = MAVLINK_STX; + buf[1] = length; + buf[2] = status->current_tx_seq; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + status->current_tx_seq++; + checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&checksum, packet, length); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); +#endif + ck[0] = (uint8_t)(checksum & 0xFF); + ck[1] = (uint8_t)(checksum >> 8); + + MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); + _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); + _mavlink_send_uart(chan, packet, length); + _mavlink_send_uart(chan, (const char *)ck, 2); + MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +/** + * @brief Pack a message to send it over a serial byte stream + */ +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) +{ + memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; +} + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->checksum); +} + +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->checksum); +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; + + /* + default message crc function. You can override this per-system to + put this data in a different memory segment + */ +#if MAVLINK_CRC_EXTRA +#ifndef MAVLINK_MESSAGE_CRC + static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] +#endif +#endif + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + _MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: +#if MAVLINK_CRC_EXTRA + mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); +#endif + if (c != (rxmsg->checksum & 0xFF)) { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != (rxmsg->checksum >> 8)) { + // Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; + return status->msg_received; +} + +/** + * @brief Put a bitfield of length 1-32 bit into the buffer + * + * @param b the value to add, will be encoded in the bitfield + * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. + * @param packet_index the position in the packet (the index of the first byte to use) + * @param bit_index the position in the byte (the index of the first bit to use) + * @param buffer packet buffer to write into + * @return new position of the last used byte in the buffer + */ +MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) +{ + uint16_t bits_remain = bits; + // Transform number into network order + int32_t v; + uint8_t i_bit_index, i_byte_index, curr_bits_n; +#if MAVLINK_NEED_BYTE_SWAP + union { + int32_t i; + uint8_t b[4]; + } bin, bout; + bin.i = b; + bout.b[0] = bin.b[3]; + bout.b[1] = bin.b[2]; + bout.b[2] = bin.b[1]; + bout.b[3] = bin.b[0]; + v = bout.i; +#else + v = b; +#endif + + // buffer in + // 01100000 01000000 00000000 11110001 + // buffer out + // 11110001 00000000 01000000 01100000 + + // Existing partly filled byte (four free slots) + // 0111xxxx + + // Mask n free bits + // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 + // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 + + // Shift n bits into the right position + // out = in >> n; + + // Mask and shift bytes + i_bit_index = bit_index; + i_byte_index = packet_index; + if (bit_index > 0) + { + // If bits were available at start, they were available + // in the byte before the current index + i_byte_index--; + } + + // While bits have not been packed yet + while (bits_remain > 0) + { + // Bits still have to be packed + // there can be more than 8 bits, so + // we might have to pack them into more than one byte + + // First pack everything we can into the current 'open' byte + //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 + //FIXME + if (bits_remain <= (uint8_t)(8 - i_bit_index)) + { + // Enough space + curr_bits_n = bits_remain; + } + else + { + curr_bits_n = (8 - i_bit_index); + } + + // Pack these n bits into the current byte + // Mask out whatever was at that position with ones (xxx11111) + buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); + // Put content to this position, by masking out the non-used part + buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v); + + // Increment the bit index + i_bit_index += curr_bits_n; + + // Now proceed to the next byte, if necessary + bits_remain -= curr_bits_n; + if (bits_remain > 0) + { + // Offer another 8 bits / one byte + i_byte_index++; + i_bit_index = 0; + } + } + + *r_bit_index = i_bit_index; + // If a partly filled byte is present, mark this as consumed + if (i_bit_index != 7) i_byte_index++; + return i_byte_index - packet_index; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define comm_send_ch() if you wish +// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a +// whole packet at a time + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + */ + +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) +{ +#ifdef MAVLINK_SEND_UART_BYTES + /* this is the more efficient approach, if the platform + defines it */ + MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len); +#else + /* fallback to one byte at a time */ + uint16_t i; + for (i = 0; i < len; i++) { + comm_send_ch(chan, (uint8_t)buf[i]); + } +#endif +} +#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS + +#endif /* _MAVLINK_HELPERS_H_ */ diff --git a/libraries/GCS_MAVLink/include/mavlink_types.h b/libraries/GCS_MAVLink/include/mavlink_types.h index 67eed17c8e..7582afd948 100644 --- a/libraries/GCS_MAVLink/include/mavlink_types.h +++ b/libraries/GCS_MAVLink/include/mavlink_types.h @@ -9,12 +9,12 @@ enum MAV_CLASS MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com - MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org - MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints - MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands - MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set - MAV_CLASS_NONE = 8, ///< No valid autopilot - MAV_CLASS_NB ///< Number of autopilot classes + MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands + MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set + MAV_CLASS_NONE = 8, ///< No valid autopilot + MAV_CLASS_NB ///< Number of autopilot classes }; enum MAV_ACTION @@ -60,8 +60,8 @@ enum MAV_ACTION MAV_ACTION_CHANGE_MODE = 38, MAV_ACTION_LOITER_MAX_TURNS = 39, MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, + MAV_ACTION_START_HILSIM = 41, + MAV_ACTION_STOP_HILSIM = 42, MAV_ACTION_NB ///< Number of MAV actions }; @@ -103,7 +103,7 @@ enum MAV_NAV MAV_NAV_LANDING, MAV_NAV_LOST, MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT + MAV_NAV_FREE_DRIFT }; enum MAV_TYPE @@ -115,11 +115,11 @@ enum MAV_TYPE MAV_HELICOPTER = 4, MAV_GROUND = 5, OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 + MAV_AIRSHIP = 7, + MAV_FREE_BALLOON = 8, + MAV_ROCKET = 9, + UGV_GROUND_ROVER = 10, + UGV_SURFACE_SHIP = 11 }; enum MAV_AUTOPILOT_TYPE @@ -128,7 +128,7 @@ enum MAV_AUTOPILOT_TYPE MAV_AUTOPILOT_PIXHAWK = 1, MAV_AUTOPILOT_SLUGS = 2, MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 + MAV_AUTOPILOT_NONE = 4 }; enum MAV_COMPONENT @@ -141,8 +141,8 @@ enum MAV_COMPONENT MAV_COMP_ID_MAPPER, MAV_COMP_ID_CAMERA, MAV_COMP_ID_IMU = 200, - MAV_COMP_ID_IMU_2 = 201, - MAV_COMP_ID_IMU_3 = 202, + MAV_COMP_ID_IMU_2 = 201, + MAV_COMP_ID_IMU_3 = 202, MAV_COMP_ID_UDP_BRIDGE = 240, MAV_COMP_ID_UART_BRIDGE = 241, MAV_COMP_ID_SYSTEM_CONTROL = 250 @@ -153,33 +153,37 @@ enum MAV_FRAME MAV_FRAME_GLOBAL = 0, MAV_FRAME_LOCAL = 1, MAV_FRAME_MISSION = 2, - MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, - MAV_FRAME_LOCAL_ENU = 4 + MAV_FRAME_GLOBAL_RELATIVE_ALT = 3, + MAV_FRAME_LOCAL_ENU = 4 }; enum MAVLINK_DATA_STREAM_TYPE { MAVLINK_DATA_STREAM_IMG_JPEG, - MAVLINK_DATA_STREAM_IMG_BMP, - MAVLINK_DATA_STREAM_IMG_RAW8U, - MAVLINK_DATA_STREAM_IMG_RAW32U, - MAVLINK_DATA_STREAM_IMG_PGM, - MAVLINK_DATA_STREAM_IMG_PNG - + MAVLINK_DATA_STREAM_IMG_BMP, + MAVLINK_DATA_STREAM_IMG_RAW8U, + MAVLINK_DATA_STREAM_IMG_RAW32U, + MAVLINK_DATA_STREAM_IMG_PGM, + MAVLINK_DATA_STREAM_IMG_PNG }; -#define MAVLINK_STX 0x55 ///< Packet start sign -#define MAVLINK_STX_LEN 1 ///< Length of start sign #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length #define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum #define MAVLINK_NUM_CHECKSUM_BYTES 2 #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) #define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length -//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct param_union { + struct { + float param_float; + int32_t param_int32; + uint32_t param_uint32; + }; + uint8_t type; +} mavlink_param_union_t; typedef struct __mavlink_system { uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function @@ -187,20 +191,58 @@ typedef struct __mavlink_system { uint8_t type; ///< Unused, can be used by user to store the system's type uint8_t state; ///< Unused, can be used by user to store the system's state uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode } mavlink_system_t; typedef struct __mavlink_message { - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte + uint16_t checksum; /// sent at end of packet + uint8_t magic; ///< protocol magic marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; +typedef enum { + MAVLINK_TYPE_CHAR = 0, + MAVLINK_TYPE_UINT8_T = 1, + MAVLINK_TYPE_INT8_T = 2, + MAVLINK_TYPE_UINT16_T = 3, + MAVLINK_TYPE_INT16_T = 4, + MAVLINK_TYPE_UINT32_T = 5, + MAVLINK_TYPE_INT32_T = 6, + MAVLINK_TYPE_UINT64_T = 7, + MAVLINK_TYPE_INT64_T = 8, + MAVLINK_TYPE_FLOAT = 9, + MAVLINK_TYPE_DOUBLE = 10 +} mavlink_message_type_t; + +#define MAVLINK_MAX_FIELDS 64 + +typedef struct __mavlink_field_info { + const char *name; // name of this field + mavlink_message_type_t type; // type of this field + unsigned array_length; // if non-zero, field is an array + unsigned wire_offset; // offset of each field in the payload + unsigned structure_offset; // offset in a C structure +} mavlink_field_info_t; + +// note that in this structure the order of fields is the order +// in the XML file, not necessary the wire order +typedef struct __mavlink_message_info { + const char *name; // name of the message + unsigned num_fields; // how many fields in this message + const mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information +} mavlink_message_info_t; + +#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0])) + +// checksum is immediately after the payload bytes +#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg)) +#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg)) + typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, @@ -235,8 +277,6 @@ typedef enum { } mavlink_parse_state_t; ///< The state machine for the comm parser typedef struct __mavlink_status { - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 uint8_t msg_received; ///< Number of received messages uint8_t buffer_overrun; ///< Number of buffer overruns uint8_t parse_error; ///< Number of parse errors @@ -248,4 +288,7 @@ typedef struct __mavlink_status { uint16_t packet_rx_drop_count; ///< Number of packet drops } mavlink_status_t; +#define MAVLINK_BIG_ENDIAN 0 +#define MAVLINK_LITTLE_ENDIAN 1 + #endif /* MAVLINK_TYPES_H_ */ diff --git a/libraries/GCS_MAVLink/include/protocol.h b/libraries/GCS_MAVLink/include/protocol.h index ed1f9546b4..7b306d2a92 100644 --- a/libraries/GCS_MAVLink/include/protocol.h +++ b/libraries/GCS_MAVLink/include/protocol.h @@ -2,121 +2,70 @@ #define _MAVLINK_PROTOCOL_H_ #include "string.h" -#include "checksum.h" - #include "mavlink_types.h" +/* + If you want MAVLink on a system that is native big-endian, + you need to define NATIVE_BIG_ENDIAN +*/ +#ifdef NATIVE_BIG_ENDIAN +# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN) +#else +# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN) +#endif -/** - * @brief Initialize the communication stack - * - * This function has to be called before using commParseBuffer() to initialize the different status registers. - * - * @return Will initialize the different buffers and status registers. - */ -static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) -{ - if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) - { - initStatus->ck_a = 0; - initStatus->ck_b = 0; - initStatus->msg_received = 0; - initStatus->buffer_overrun = 0; - initStatus->parse_error = 0; - initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; - initStatus->packet_idx = 0; - initStatus->packet_rx_drop_count = 0; - initStatus->packet_rx_success_count = 0; - initStatus->current_rx_seq = 0; - initStatus->current_tx_seq = 0; - } -} +#ifndef MAVLINK_STACK_BUFFER +#define MAVLINK_STACK_BUFFER 0 +#endif -static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - return &m_mavlink_status[chan]; -} +#ifndef MAVLINK_AVOID_GCC_STACK_BUG +# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__) +#endif -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. - * - * @warning This function implicitely assumes the message is sent over channel zero. - * if the message is sent over a different channel it will reach the receiver - * without error, BUT the sequence number might be wrong due to the wrong - * channel sequence counter. This will result is wrongly reported excessive - * packet loss. Please use @see mavlink_{pack|encode}_headerless and then - * @see mavlink_finalize_message_chan before sending for a correct channel - * assignment. Please note that the mavlink_msg_xxx_pack and encode functions - * assign channel zero as default and thus induce possible loss counter errors.\ - * They have been left to ensure code compatibility. - * - * @see mavlink_finalize_message_chan - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length, usually just the counter incremented while packing the message - */ -static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); - msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte - msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte +#ifndef MAVLINK_ASSERT +#define MAVLINK_ASSERT(x) +#endif - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; -} +#ifndef MAVLINK_START_UART_SEND +#define MAVLINK_START_UART_SEND(chan, length) +#endif -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length, usually just the counter incremented while packing the message - */ -static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); - msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte - msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte +#ifndef MAVLINK_END_UART_SEND +#define MAVLINK_END_UART_SEND(chan, length) +#endif - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; -} +#ifdef MAVLINK_SEPARATE_HELPERS +#define MAVLINK_HELPER +#else +#define MAVLINK_HELPER static inline +#include "mavlink_helpers.h" +#endif // MAVLINK_SEPARATE_HELPERS -/** - * @brief Pack a message to send it over a serial byte stream - */ -static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) -{ - *(buffer+0) = MAVLINK_STX; ///< Start transmit - memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - return 0; -} +/* always include the prototypes to ensure we don't get out of sync */ +MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan); +#if MAVLINK_CRC_EXTRA +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint16_t length, uint8_t crc_extra); +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint16_t length, uint8_t crc_extra); +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, + uint8_t length, uint8_t crc_extra); +#else +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint16_t length); +MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint16_t length); +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); +#endif // MAVLINK_CRC_EXTRA +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg); +MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg); +MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c); +MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status); +MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, + uint8_t* r_bit_index, uint8_t* buffer); +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); +#endif /** * @brief Get the required buffer size for this message @@ -126,857 +75,248 @@ static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_ return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; } -union checksum_ { - uint16_t s; - uint8_t c[2]; -}; - -union __mavlink_bitfield { - uint8_t uint8; - int8_t int8; - uint16_t uint16; - int16_t int16; - uint32_t uint32; - int32_t int32; -}; - - -static inline void mavlink_start_checksum(mavlink_message_t* msg) +#if MAVLINK_NEED_BYTE_SWAP +static inline void byte_swap_2(char *dst, const char *src) { - union checksum_ ck; - crc_init(&(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; + dst[0] = src[1]; + dst[1] = src[0]; } - -static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +static inline void byte_swap_4(char *dst, const char *src) { - union checksum_ ck; - ck.c[0] = msg->ck_a; - ck.c[1] = msg->ck_b; - crc_accumulate(c, &(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; } - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +static inline void byte_swap_8(char *dst, const char *src) { - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - - // Initializes only once, values keep unchanged after first initialization - mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); - - mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - rxmsg->payload[status->packet_idx++] = c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: - if (c != rxmsg->ck_a) - { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != rxmsg->ck_b) - {// Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - return status->msg_received; + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; } - - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ - -#define MAVLINK_PACKET_START_CANDIDATES 50 -/* -static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#elif !MAVLINK_ALIGNED_FIELDS +static inline void byte_copy_2(char *dst, const char *src) { - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_msgbuf[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_MAX_PACKET_LEN * 2]; - static uint8_t m_msgbuf_index[MAVLINK_COMM_NUM_BUFFERS]; - static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_packet_start[MAVLINK_COMM_NUM_BUFFERS][MAVLINK_PACKET_START_CANDIDATES]; - static uint8_t m_packet_start_index_read[MAVLINK_COMM_NUM_BUFFERS]; - static uint8_t m_packet_start_index_write[MAVLINK_COMM_NUM_BUFFERS]; - - // Set a packet start candidate index if sign is start sign - if (c == MAVLINK_STX) - { - m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; - } - - // Parse normally, if a CRC mismatch occurs retry with the next packet index + dst[0] = src[0]; + dst[1] = src[1]; } -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; -//// Initializes only once, values keep unchanged after first initialization -// mavlink_parse_state_initialize(&m_mavlink_status[chan]); -// -//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message -//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status -//int bufferIndex = 0; -// -//status->msg_received = 0; -// -//switch (status->parse_state) -//{ -//case MAVLINK_PARSE_STATE_UNINIT: -//case MAVLINK_PARSE_STATE_IDLE: -// if (c == MAVLINK_STX) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; -// mavlink_start_checksum(rxmsg); -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_STX: -// if (status->msg_received) -// { -// status->buffer_overrun++; -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 -// rxmsg->len = c; -// status->packet_idx = 0; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_LENGTH: -// rxmsg->seq = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SEQ: -// rxmsg->sysid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_SYSID: -// rxmsg->compid = c; -// mavlink_update_checksum(rxmsg, c); -// status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; -// break; -// -//case MAVLINK_PARSE_STATE_GOT_COMPID: -// rxmsg->msgid = c; -// mavlink_update_checksum(rxmsg, c); -// if (rxmsg->len == 0) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// else -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_MSGID: -// rxmsg->payload[status->packet_idx++] = c; -// mavlink_update_checksum(rxmsg, c); -// if (status->packet_idx == rxmsg->len) -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -// if (c != rxmsg->ck_a) -// { -// // Check first checksum byte -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; -// } -// break; -// -//case MAVLINK_PARSE_STATE_GOT_CRC1: -// if (c != rxmsg->ck_b) -// {// Check second checksum byte -// status->parse_error++; -// status->msg_received = 0; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// } -// else -// { -// // Successfully got message -// status->msg_received = 1; -// status->parse_state = MAVLINK_PARSE_STATE_IDLE; -// memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); -// } -// break; -//} - -bufferIndex++; -// If a message has been sucessfully decoded, check index -if (status->msg_received == 1) +static inline void byte_copy_4(char *dst, const char *src) { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; } - -r_mavlink_status->current_seq = status->current_seq+1; -r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; -r_mavlink_status->packet_rx_drop_count = status->parse_error; -return status->msg_received; -} - */ - - -typedef union __generic_16bit +static inline void byte_copy_8(char *dst, const char *src) { - uint8_t b[2]; - int16_t s; -} generic_16bit; - -typedef union __generic_32bit -{ - uint8_t b[4]; - float f; - int32_t i; - int16_t s; -} generic_32bit; - -typedef union __generic_64bit -{ - uint8_t b[8]; - int64_t ll; ///< Long long (64 bit) - double d; ///< IEEE-754 double precision floating point -} generic_64bit; - -/** - * @brief Place an unsigned byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = b; - return sizeof(b); -} - -/** - * @brief Place a signed byte into the buffer - * - * @param b the byte to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) -{ - *(buffer + bindex) = (uint8_t)b; - return sizeof(b); -} - -/** - * @brief Place two unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>8)&0xff; - buffer[bindex+1] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place two signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint16_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>24)&0xff; - buffer[bindex+1] = (b>>16)&0xff; - buffer[bindex+2] = (b>>8)&0xff; - buffer[bindex+3] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) -{ - buffer[bindex] = (b>>56)&0xff; - buffer[bindex+1] = (b>>48)&0xff; - buffer[bindex+2] = (b>>40)&0xff; - buffer[bindex+3] = (b>>32)&0xff; - buffer[bindex+4] = (b>>24)&0xff; - buffer[bindex+5] = (b>>16)&0xff; - buffer[bindex+6] = (b>>8)&0xff; - buffer[bindex+7] = (b & 0xff); - return sizeof(b); -} - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) -{ - return put_uint64_t_by_index(b, bindex, buffer); -} - -/** - * @brief Place a float into the buffer - * - * @param b the float to add - * @param bindex the position in the packet - * @param buffer the packet buffer - * @return the new position of the last used byte in the buffer - */ -static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) -{ - generic_32bit g; - g.f = b; - return put_int32_t_by_index(g.i, bindex, buffer); -} - -/** - * @brief Place an array into the buffer - * - * @param b the array to add - * @param length size of the array (for strings: length WITH '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) -{ - memcpy(buffer+bindex, b, length); - return length; -} - -/** - * @brief Place a string into the buffer - * - * @param b the string to add - * @param maxlength size of the array (for strings: length WITHOUT '\0' char) - * @param bindex the position in the packet - * @param buffer packet buffer - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) -{ - uint16_t length = 0; - // Copy string into buffer, ensuring not to exceed the buffer size - int i; - for (i = 1; i < maxlength; i++) - { - length++; - // String characters - if (i < (maxlength - 1)) - { - buffer[bindex+i] = b[i]; - // Stop at null character - if (b[i] == '\0') - { - break; - } - } - // Enforce null termination at end of buffer - else if (i == (maxlength - 1)) - { - buffer[i] = '\0'; - } - } - // Write length into first field - put_uint8_t_by_index(length, bindex, buffer); - return length; -} - -/** - * @brief Put a bitfield of length 1-32 bit into the buffer - * - * @param b the value to add, will be encoded in the bitfield - * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. - * @param packet_index the position in the packet (the index of the first byte to use) - * @param bit_index the position in the byte (the index of the first bit to use) - * @param buffer packet buffer to write into - * @return new position of the last used byte in the buffer - */ -static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) -{ - uint16_t bits_remain = bits; - // Transform number into network order - generic_32bit bin; - generic_32bit bout; - uint8_t i_bit_index, i_byte_index, curr_bits_n; - bin.i = b; - bout.b[0] = bin.b[3]; - bout.b[1] = bin.b[2]; - bout.b[2] = bin.b[1]; - bout.b[3] = bin.b[0]; - - // buffer in - // 01100000 01000000 00000000 11110001 - // buffer out - // 11110001 00000000 01000000 01100000 - - // Existing partly filled byte (four free slots) - // 0111xxxx - - // Mask n free bits - // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 - // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 - - // Shift n bits into the right position - // out = in >> n; - - // Mask and shift bytes - i_bit_index = bit_index; - i_byte_index = packet_index; - if (bit_index > 0) - { - // If bits were available at start, they were available - // in the byte before the current index - i_byte_index--; - } - - // While bits have not been packed yet - while (bits_remain > 0) - { - // Bits still have to be packed - // there can be more than 8 bits, so - // we might have to pack them into more than one byte - - // First pack everything we can into the current 'open' byte - //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 - //FIXME - if (bits_remain <= (8 - i_bit_index)) - { - // Enough space - curr_bits_n = bits_remain; - } - else - { - curr_bits_n = (8 - i_bit_index); - } - - // Pack these n bits into the current byte - // Mask out whatever was at that position with ones (xxx11111) - buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); - // Put content to this position, by masking out the non-used part - buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); - - // Increment the bit index - i_bit_index += curr_bits_n; - - // Now proceed to the next byte, if necessary - bits_remain -= curr_bits_n; - if (bits_remain > 0) - { - // Offer another 8 bits / one byte - i_byte_index++; - i_bit_index = 0; - } - } - - *r_bit_index = i_bit_index; - // If a partly filled byte is present, mark this as consumed - if (i_bit_index != 7) i_byte_index++; - return i_byte_index - packet_index; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define a similar function - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - */ - -static inline void mavlink_send_uart_uint8_t(mavlink_channel_t chan, uint8_t b, uint16_t* checksum) -{ - crc_accumulate(b, checksum); - comm_send_ch(chan, b); -} - -static inline void mavlink_send_uart_int8_t(mavlink_channel_t chan, int8_t b, uint16_t* checksum) -{ - crc_accumulate(b, checksum); - comm_send_ch(chan, b); -} - -static inline void mavlink_send_uart_uint16_t(mavlink_channel_t chan, uint16_t b, uint16_t* checksum) -{ - char s; - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int16_t(mavlink_channel_t chan, int16_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint16_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_uint32_t(mavlink_channel_t chan, uint32_t b, uint16_t* checksum) -{ - char s; - s = (b>>24)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>16)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int32_t(mavlink_channel_t chan, int32_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint32_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_uint64_t(mavlink_channel_t chan, uint64_t b, uint16_t* checksum) -{ - char s; - s = (b>>56)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>48)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>40)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>32)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>24)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>16)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b>>8)&0xff; - comm_send_ch(chan, s); - crc_accumulate(s, checksum); - s = (b & 0xff); - comm_send_ch(chan, s); - crc_accumulate(s, checksum); -} - -static inline void mavlink_send_uart_int64_t(mavlink_channel_t chan, int64_t b, uint16_t* checksum) -{ - mavlink_send_uart_uint64_t(chan, b, checksum); -} - -static inline void mavlink_send_uart_float(mavlink_channel_t chan, float b, uint16_t* checksum) -{ - generic_32bit g; - g.f = b; - mavlink_send_uart_uint32_t(chan, g.i, checksum); -} - -static inline void mavlink_send_uart_double(mavlink_channel_t chan, double b, uint16_t* checksum) -{ - generic_64bit g; - g.d = b; - mavlink_send_uart_uint32_t(chan, g.ll, checksum); -} - -static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) -{ - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - comm_send_ch(chan, MAVLINK_STX); - comm_send_ch(chan, msg->len); - comm_send_ch(chan, msg->seq); - comm_send_ch(chan, msg->sysid); - comm_send_ch(chan, msg->compid); - comm_send_ch(chan, msg->msgid); - for(uint16_t i = 0; i < msg->len; i++) - { - comm_send_ch(chan, msg->payload[i]); - } - comm_send_ch(chan, msg->ck_a); - comm_send_ch(chan, msg->ck_b); + memcpy(dst, src, 8); } #endif -#endif /* _MAVLINK_PROTOCOL_H_ */ +#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b +#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b +#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b + +#if MAVLINK_NEED_BYTE_SWAP +#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#elif !MAVLINK_ALIGNED_FIELDS +#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#else +#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b +#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b +#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b +#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b +#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b +#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b +#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b +#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b +#endif + + +/* + * Place a char array into a buffer + */ +static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) +{ + if (b == NULL) { + memset(&buf[wire_offset], 0, array_length); + } else { + memcpy(&buf[wire_offset], b, array_length); + } +} + +/* + * Place a uint8_t array into a buffer + */ +static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) +{ + if (b == NULL) { + memset(&buf[wire_offset], 0, array_length); + } else { + memcpy(&buf[wire_offset], b, array_length); + } +} + +/* + * Place a int8_t array into a buffer + */ +static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) +{ + if (b == NULL) { + memset(&buf[wire_offset], 0, array_length); + } else { + memcpy(&buf[wire_offset], b, array_length); + } +} + +#if MAVLINK_NEED_BYTE_SWAP +#define _MAV_PUT_ARRAY(TYPE, V) \ +static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +{ \ + if (b == NULL) { \ + memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ + } else { \ + uint16_t i; \ + for (i=0; i