From 946acc189a88b00079a76a9c7ae0821e113aad80 Mon Sep 17 00:00:00 2001 From: lm Date: Sun, 31 Jul 2011 17:22:29 +0200 Subject: [PATCH] Updated to MAVLink v1.0 --- .../include/ardupilotmega/ardupilotmega.h | 9 +- .../mavlink/include/ardupilotmega/mavlink.h | 8 +- thirdParty/mavlink/include/checksum.h | 44 + thirdParty/mavlink/include/common/common.h | 203 +- thirdParty/mavlink/include/common/mavlink.h | 8 +- .../include/common/mavlink_msg_action.h | 92 +- .../include/common/mavlink_msg_action_ack.h | 82 +- .../include/common/mavlink_msg_attitude.h | 171 +- .../mavlink_msg_attitude_controller_output.h | 112 +- .../include/common/mavlink_msg_auth_key.h | 76 +- .../mavlink/include/common/mavlink_msg_boot.h | 77 +- .../mavlink_msg_change_operator_control.h | 106 +- .../mavlink_msg_change_operator_control_ack.h | 92 +- .../include/common/mavlink_msg_command.h | 162 +- .../include/common/mavlink_msg_command_ack.h | 92 +- .../common/mavlink_msg_control_status.h | 142 +- .../include/common/mavlink_msg_debug.h | 87 +- .../include/common/mavlink_msg_debug_vect.h | 140 +- .../common/mavlink_msg_global_position.h | 171 +- .../common/mavlink_msg_global_position_int.h | 146 +- .../common/mavlink_msg_gps_local_origin_set.h | 107 +- .../include/common/mavlink_msg_gps_raw.h | 196 +- .../include/common/mavlink_msg_gps_raw_int.h | 196 +- .../mavlink_msg_gps_set_global_origin.h | 127 +- .../include/common/mavlink_msg_gps_status.h | 158 +- .../include/common/mavlink_msg_heartbeat.h | 92 +- .../common/mavlink_msg_local_position.h | 171 +- .../mavlink_msg_local_position_setpoint.h | 122 +- .../mavlink_msg_local_position_setpoint_set.h | 142 +- .../common/mavlink_msg_manual_control.h | 172 +- .../common/mavlink_msg_named_value_float.h | 91 +- .../common/mavlink_msg_named_value_int.h | 91 +- .../mavlink_msg_nav_controller_output.h | 176 +- .../common/mavlink_msg_param_request_list.h | 82 +- .../common/mavlink_msg_param_request_read.h | 117 +- .../include/common/mavlink_msg_param_set.h | 119 +- .../include/common/mavlink_msg_param_value.h | 125 +- .../mavlink/include/common/mavlink_msg_ping.h | 116 +- .../mavlink_msg_position_controller_output.h | 112 +- .../common/mavlink_msg_position_target.h | 122 +- .../include/common/mavlink_msg_raw_imu.h | 202 +- .../include/common/mavlink_msg_raw_pressure.h | 133 +- .../common/mavlink_msg_rc_channels_raw.h | 176 +- .../common/mavlink_msg_rc_channels_scaled.h | 176 +- .../common/mavlink_msg_request_data_stream.h | 115 +- .../common/mavlink_msg_safety_allowed_area.h | 162 +- .../mavlink_msg_safety_set_allowed_area.h | 182 +- .../include/common/mavlink_msg_scaled_imu.h | 202 +- .../common/mavlink_msg_scaled_pressure.h | 124 +- .../common/mavlink_msg_servo_output_raw.h | 166 +- .../include/common/mavlink_msg_set_altitude.h | 87 +- .../include/common/mavlink_msg_set_mode.h | 82 +- .../include/common/mavlink_msg_set_nav_mode.h | 82 +- .../common/mavlink_msg_state_correction.h | 197 +- .../include/common/mavlink_msg_statustext.h | 94 +- .../include/common/mavlink_msg_sys_status.h | 144 +- .../include/common/mavlink_msg_system_time.h | 81 +- .../common/mavlink_msg_system_time_utc.h | 92 +- .../include/common/mavlink_msg_vfr_hud.h | 148 +- .../include/common/mavlink_msg_waypoint.h | 244 +- .../include/common/mavlink_msg_waypoint_ack.h | 92 +- .../common/mavlink_msg_waypoint_clear_all.h | 82 +- .../common/mavlink_msg_waypoint_count.h | 95 +- .../common/mavlink_msg_waypoint_current.h | 75 +- .../common/mavlink_msg_waypoint_reached.h | 75 +- .../common/mavlink_msg_waypoint_request.h | 95 +- .../mavlink_msg_waypoint_request_list.h | 82 +- .../common/mavlink_msg_waypoint_set_current.h | 95 +- thirdParty/mavlink/include/mavlink_types.h | 198 +- thirdParty/mavlink/include/minimal/mavlink.h | 8 +- .../include/minimal/mavlink_msg_heartbeat.h | 92 +- thirdParty/mavlink/include/minimal/minimal.h | 9 +- thirdParty/mavlink/include/pixhawk/mavlink.h | 8 +- .../pixhawk/mavlink_msg_attitude_control.h | 172 +- .../include/pixhawk/mavlink_msg_aux_status.h | 140 +- .../pixhawk/mavlink_msg_brief_feature.h | 180 +- .../mavlink_msg_data_transmission_handshake.h | 117 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 89 +- .../pixhawk/mavlink_msg_image_available.h | 407 ++-- .../mavlink_msg_image_trigger_control.h | 72 +- .../pixhawk/mavlink_msg_image_triggered.h | 250 ++- .../include/pixhawk/mavlink_msg_marker.h | 165 +- .../pixhawk/mavlink_msg_pattern_detected.h | 119 +- .../pixhawk/mavlink_msg_point_of_interest.h | 172 +- ...mavlink_msg_point_of_interest_connection.h | 221 +- .../mavlink_msg_position_control_offset_set.h | 142 +- .../mavlink_msg_position_control_setpoint.h | 135 +- ...avlink_msg_position_control_setpoint_set.h | 155 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 155 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 133 +- .../mavlink_msg_vicon_position_estimate.h | 171 +- .../mavlink_msg_vision_position_estimate.h | 171 +- .../mavlink_msg_vision_speed_estimate.h | 126 +- .../pixhawk/mavlink_msg_watchdog_command.h | 108 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 88 +- .../mavlink_msg_watchdog_process_info.h | 141 +- .../mavlink_msg_watchdog_process_status.h | 136 +- thirdParty/mavlink/include/pixhawk/pixhawk.h | 11 +- thirdParty/mavlink/include/protocol.h | 299 ++- thirdParty/mavlink/include/slugs/mavlink.h | 8 +- .../include/slugs/mavlink_msg_air_data.h | 105 +- .../include/slugs/mavlink_msg_cpu_load.h | 95 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 85 +- .../include/slugs/mavlink_msg_data_log.h | 152 +- .../include/slugs/mavlink_msg_diagnostic.h | 146 +- .../include/slugs/mavlink_msg_gps_date_time.h | 132 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 117 +- .../include/slugs/mavlink_msg_sensor_bias.h | 152 +- .../include/slugs/mavlink_msg_slugs_action.h | 95 +- .../slugs/mavlink_msg_slugs_navigation.h | 187 +- thirdParty/mavlink/include/slugs/slugs.h | 9 +- thirdParty/mavlink/include/ualberta/mavlink.h | 8 +- .../ualberta/mavlink_msg_nav_filter_bias.h | 171 +- .../ualberta/mavlink_msg_radio_calibration.h | 146 +- .../mavlink_msg_request_rc_channels.h | 101 - .../mavlink_msg_ualberta_sys_status.h | 92 +- .../mavlink/include/ualberta/ualberta.h | 15 +- .../mavlink/message_definitions/common.xml | 1997 +++++++++-------- .../mavlink/message_definitions/pixhawk.xml | 12 +- .../mavlink/missionlib/testing/.gitignore | 2 + thirdParty/mavlink/missionlib/testing/udp.c | 213 ++ 121 files changed, 10424 insertions(+), 6240 deletions(-) delete mode 100644 thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h create mode 100644 thirdParty/mavlink/missionlib/testing/.gitignore create mode 100644 thirdParty/mavlink/missionlib/testing/udp.c diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 4d13e0b54..d03060c3d 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -33,13 +33,6 @@ extern "C" { // MESSAGE DEFINITIONS - - -// 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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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 diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 8ee2430d7..ba99d28cc 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "ardupilotmega.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/checksum.h b/thirdParty/mavlink/include/checksum.h index c16a06e1b..07bab9102 100644 --- a/thirdParty/mavlink/include/checksum.h +++ b/thirdParty/mavlink/include/checksum.h @@ -34,6 +34,7 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) tmp=data ^ (uint8_t)(*crcAccum &0xff); tmp^= (tmp<<4); *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test } /** @@ -86,6 +87,49 @@ static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) } +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int 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=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &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); +} + + #endif /* _CHECKSUM_H_ */ diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index bf99ef000..39740c7af 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef COMMON_H #define COMMON_H @@ -18,17 +18,180 @@ extern "C" { // MAVLINK VERSION #ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 +#define MAVLINK_VERSION 3 #endif #if (MAVLINK_VERSION == 0) #undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 +#define MAVLINK_VERSION 3 #endif // ENUM DEFINITIONS -/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +enum MAV_CLASS +{ + MAV_CLASS_GENERIC=0, /* Generic autopilot, full support for everything | */ + 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_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_CLASS_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_CLASS_UDB=10, /* UAV Dev Board | */ + MAV_CLASS_FP=11, /* FlexiPilot | */ + MAV_CLASS_ENUM_END +}; + +/** @brief */ +enum MAV_MODE +{ + MAV_MODE_UNINIT=0, /* System is in undefined state | */ + MAV_MODE_LOCKED=1, /* Motors are blocked, system is safe | */ + MAV_MODE_MANUAL=2, /* System is allowed to be active, under manual (RC) control | */ + MAV_MODE_GUIDED=3, /* System is allowed to be active, under autonomous control, manual setpoint | */ + MAV_MODE_AUTO=4, /* System is allowed to be active, under autonomous control and navigation | */ + MAV_MODE_TEST1=5, /* Generic test mode, for custom use | */ + MAV_MODE_TEST2=6, /* Generic test mode, for custom use | */ + MAV_MODE_TEST3=7, /* Generic test mode, for custom use | */ + MAV_MODE_READY=8, /* System is ready, motors are unblocked, but controllers are inactive | */ + MAV_MODE_RC_TRAINING=9, /* System is blocked, only RC valued are read and reported back | */ + MAV_MODE_ENUM_END +}; + +/** @brief */ +enum MAV_NAV +{ + MAV_NAV_GROUNDED=0, /* System is currently on ground. | */ + MAV_NAV_LIFTOFF=1, /* System is during liftoff, not in normal navigation mode yet. | */ + MAV_NAV_HOLD=2, /* System is holding its current position (rotorcraft or rover / boat). | */ + MAV_NAV_WAYPOINT=3, /* System is navigating towards the next waypoint. | */ + MAV_NAV_VECTOR=4, /* System is flying at a defined course and speed. | */ + MAV_NAV_RETURNING=5, /* System is return straight to home position. | */ + MAV_NAV_LANDING=6, /* System is landing. | */ + MAV_NAV_LOST=7, /* System lost its position input and is in attitude / flight stabilization only. | */ + MAV_NAV_LOITER=8, /* System is loitering in wait position. DO NOT USE THIS FOR WAYPOINT LOITER! | */ + MAV_NAV_ENUM_END +}; + +/** @brief */ +enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_ENUM_END +}; + +/** @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 | */ + MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ + MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ + MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ + MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ + MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ + MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ + MAV_DATA_STREAM_ENUM_END +}; + +/** @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_WPNEXT=0, /* Point toward next waypoint. | */ + MAV_ROI_WPINDEX=1, /* Point toward given waypoint. | */ + MAV_ROI_LOCATION=2, /* Point toward fixed location. | */ + MAV_ROI_TARGET=3, /* Point toward of given id. | */ + MAV_ROI_ENUM_END +}; + +/** @brief */ +enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle. | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_GROUND=5, /* Ground installation | */ + MAV_TYPE_OCU=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_UGV_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_UGV_SURFACE_SHIP=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_UGV_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_ENUM_END +}; + +/** @brief */ +enum MAV_COMPONENT +{ + MAV_COMP_ID_GPS=220, + MAV_COMP_ID_WAYPOINTPLANNER=190, + MAV_COMP_ID_PATHPLANNER=195, + MAV_COMP_ID_MAPPER=180, + MAV_COMP_ID_CAMERA=100, + MAV_COMP_ID_IMU=200, + 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, + MAV_COMP_ID_SERVO1=140, + MAV_COMP_ID_SERVO2=141, + MAV_COMP_ID_SERVO3=142, + MAV_COMP_ID_SERVO4=143, + MAV_COMP_ID_SERVO5=144, + MAV_COMP_ID_SERVO6=145, + MAV_COMP_ID_SERVO7=146, + MAV_COMP_ID_SERVO8=147, + MAV_COMP_ID_SERVO9=148, + MAV_COMP_ID_SERVO10=149, + MAV_COMP_ID_SERVO11=150, + MAV_COMP_ID_SERVO12=151, + MAV_COMP_ID_SERVO13=152, + MAV_COMP_ID_SERVO14=153, + MAV_COMPONENT_ENUM_END +}; + +/** @brief */ +enum MAV_FRAME +{ + MAV_FRAME_GLOBAL=0, /* Global coordinate frame, WGS84 coordinate system. | */ + MAV_FRAME_LOCAL_NED=1, /* Local coordinate frame, Z-up (x: north, y: east, z: down). | */ + MAV_FRAME_MISSION=2, /* NOT a coordinate frame, indicates a mission command. | */ + MAV_FRAME_GLOBAL_RELATIVE_ALT=3, /* Global coordinate frame, WGS84 coordinate system, relative altitude over ground. | */ + MAV_FRAME_LOCAL_ENU=4, /* Local coordinate frame, Z-down (x: east, y: north, z: up) | */ + MAV_FRAME_ENUM_END +}; + +/** @brief */ +enum MAVLINK_DATA_STREAM_TYPE +{ + MAVLINK_DATA_STREAM_IMG_JPEG=0, + MAVLINK_DATA_STREAM_IMG_BMP=1, + MAVLINK_DATA_STREAM_IMG_RAW8U=2, + MAVLINK_DATA_STREAM_IMG_RAW32U=3, + MAVLINK_DATA_STREAM_IMG_PGM=4, + MAVLINK_DATA_STREAM_IMG_PNG=5, + MAVLINK_DATA_STREAM_TYPE_ENUM_END +}; + +/** @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 | */ @@ -62,31 +225,6 @@ enum MAV_CMD MAV_CMD_ENUM_END }; -/** @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 | */ - MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */ - MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */ - MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */ - MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */ - MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */ - MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */ - MAV_DATA_STREAM_ENUM_END -}; - -/** @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_WPNEXT=0, /* Point toward next waypoint. | */ - MAV_ROI_WPINDEX=1, /* Point toward given waypoint. | */ - MAV_ROI_LOCATION=2, /* Point toward fixed location. | */ - MAV_ROI_TARGET=3, /* Point toward of given id. | */ - MAV_ROI_ENUM_END -}; - // MESSAGE DEFINITIONS @@ -153,13 +291,6 @@ enum MAV_ROI #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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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 diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 404b970bf..ef3973fbb 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "common.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action.h b/thirdParty/mavlink/include/common/mavlink_msg_action.h index f1de54675..82ec1f50e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action.h @@ -1,6 +1,8 @@ // MESSAGE ACTION PACKING #define MAVLINK_MSG_ID_ACTION 10 +#define MAVLINK_MSG_ID_ACTION_LEN 3 +#define MAVLINK_MSG_10_LEN 3 typedef struct __mavlink_action_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_action_t } mavlink_action_t; - - /** * @brief Pack a action message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_action_t */ 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; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTION_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_action_pack(uint8_t system_id, uint8_t compon */ 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; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t: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, MAVLINK_MSG_ID_ACTION_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t comp * @param action The action id */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) { 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); + uint16_t checksum; + mavlink_action_t *p = (mavlink_action_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ACTION_LEN; + msg.msgid = MAVLINK_MSG_ID_ACTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) +{ + mavlink_header_t hdr; + mavlink_action_t payload; + uint16_t checksum; + mavlink_action_t *p = &payload; + + p->target = target; // uint8_t:The system executing the action + p->target_component = target_component; // uint8_t:The component executing the action + p->action = action; // uint8_t:The action id + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ACTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_ACTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ 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]; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -108,7 +158,8 @@ 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]; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,7 +169,8 @@ 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]; + mavlink_action_t *p = (mavlink_action_t *)&msg->payload[0]; + return (uint8_t)(p->action); } /** @@ -129,7 +181,5 @@ 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) { - 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); + memcpy( action, msg->payload, sizeof(mavlink_action_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h index 6e93449e2..bfb41a819 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h @@ -1,6 +1,8 @@ // MESSAGE ACTION_ACK PACKING #define MAVLINK_MSG_ID_ACTION_ACK 9 +#define MAVLINK_MSG_ID_ACTION_ACK_LEN 2 +#define MAVLINK_MSG_9_LEN 2 typedef struct __mavlink_action_ack_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_action_ack_t } mavlink_action_ack_t; - - /** * @brief Pack a action_ack message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_action_ack_t */ 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; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; 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 + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t: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, MAVLINK_MSG_ID_ACTION_ACK_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_action_ack_pack(uint8_t system_id, uint8_t co */ 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; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; 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 + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t: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, MAVLINK_MSG_ID_ACTION_ACK_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t * @param result 0: Action DENIED, 1: Action executed */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) { 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); + uint16_t checksum; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg.payload[0]; + + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_ACTION_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) +{ + mavlink_header_t hdr; + mavlink_action_ack_t payload; + uint16_t checksum; + mavlink_action_ack_t *p = &payload; + + p->action = action; // uint8_t:The action id + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_ACTION_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; + return (uint8_t)(p->action); } /** @@ -102,7 +150,8 @@ 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]; + mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg->payload[0]; + return (uint8_t)(p->result); } /** @@ -113,6 +162,5 @@ 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) { - action_ack->action = mavlink_msg_action_ack_get_action(msg); - action_ack->result = mavlink_msg_action_ack_get_result(msg); + memcpy( action_ack, msg->payload, sizeof(mavlink_action_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 6daba3c54..5d395fda6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE PACKING #define MAVLINK_MSG_ID_ATTITUDE 30 +#define MAVLINK_MSG_ID_ATTITUDE_LEN 32 +#define MAVLINK_MSG_30_LEN 32 typedef struct __mavlink_attitude_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_attitude_t } mavlink_attitude_t; - - /** * @brief Pack a attitude message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_attitude_t */ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp */ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float: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, MAVLINK_MSG_ID_ATTITUDE_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co * @param yawspeed Yaw angular speed (rad/s) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { 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); + uint16_t checksum; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ATTITUDE_LEN; + msg.msgid = MAVLINK_MSG_ID_ATTITUDE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_attitude_t payload; + uint16_t checksum; + mavlink_attitude_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -156,12 +201,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -171,12 +212,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -186,12 +223,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->rollspeed); } /** @@ -201,12 +234,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->pitchspeed); } /** @@ -216,12 +245,8 @@ 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; + mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; + return (float)(p->yawspeed); } /** @@ -232,11 +257,5 @@ 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) { - attitude->usec = mavlink_msg_attitude_get_usec(msg); - attitude->roll = mavlink_msg_attitude_get_roll(msg); - attitude->pitch = mavlink_msg_attitude_get_pitch(msg); - attitude->yaw = mavlink_msg_attitude_get_yaw(msg); - attitude->rollspeed = mavlink_msg_attitude_get_rollspeed(msg); - attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); - attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); + memcpy( attitude, msg->payload, sizeof(mavlink_attitude_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h index 60fa77554..081dd82d6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT 60 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN 5 +#define MAVLINK_MSG_60_LEN 5 typedef struct __mavlink_attitude_controller_output_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_attitude_controller_output_t } mavlink_attitude_controller_output_t; - - /** * @brief Pack a attitude_controller_output message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_attitude_controller_output_t */ static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { - uint16_t i = 0; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_attitude_controller_output_pack(uint8_t syste */ static inline uint16_t mavlink_msg_attitude_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { - uint16_t i = 0; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(roll, i, msg->payload); // Attitude roll: -128: -100%, 127: +100% - i += put_int8_t_by_index(pitch, i, msg->payload); // Attitude pitch: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Attitude yaw: -128: -100%, 127: +100% - i += put_int8_t_by_index(thrust, i, msg->payload); // Attitude thrust: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t sys * @param thrust Attitude thrust: -128: -100%, 127: +100% */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { mavlink_message_t msg; - mavlink_msg_attitude_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, roll, pitch, yaw, thrust); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg.payload[0]; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; + msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) +{ + mavlink_header_t hdr; + mavlink_attitude_controller_output_t payload; + uint16_t checksum; + mavlink_attitude_controller_output_t *p = &payload; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% + p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% + p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (uint8_t)(p->enabled); } /** @@ -120,7 +174,8 @@ static inline uint8_t mavlink_msg_attitude_controller_output_get_enabled(const m */ static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->roll); } /** @@ -130,7 +185,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_roll(const mavli */ static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->pitch); } /** @@ -140,7 +196,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_pitch(const mavl */ static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->yaw); } /** @@ -150,7 +207,8 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_yaw(const mavlin */ static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->thrust); } /** @@ -161,9 +219,5 @@ static inline int8_t mavlink_msg_attitude_controller_output_get_thrust(const mav */ static inline void mavlink_msg_attitude_controller_output_decode(const mavlink_message_t* msg, mavlink_attitude_controller_output_t* attitude_controller_output) { - attitude_controller_output->enabled = mavlink_msg_attitude_controller_output_get_enabled(msg); - attitude_controller_output->roll = mavlink_msg_attitude_controller_output_get_roll(msg); - attitude_controller_output->pitch = mavlink_msg_attitude_controller_output_get_pitch(msg); - attitude_controller_output->yaw = mavlink_msg_attitude_controller_output_get_yaw(msg); - attitude_controller_output->thrust = mavlink_msg_attitude_controller_output_get_thrust(msg); + memcpy( attitude_controller_output, msg->payload, sizeof(mavlink_attitude_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index 605ba7db6..f857ed91a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -1,16 +1,16 @@ // MESSAGE AUTH_KEY PACKING #define MAVLINK_MSG_ID_AUTH_KEY 7 +#define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 +#define MAVLINK_MSG_7_LEN 32 typedef struct __mavlink_auth_key_t { char key[32]; ///< key } mavlink_auth_key_t; - #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 - /** * @brief Pack a auth_key message * @param system_id ID of this system @@ -22,12 +22,12 @@ typedef struct __mavlink_auth_key_t */ 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; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); } /** @@ -41,12 +41,12 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp */ 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; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - i += put_array_by_index((const int8_t*)key, sizeof(char)*32, i, msg->payload); // key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); } /** @@ -69,12 +69,57 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co * @param key key */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - 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); + uint16_t checksum; + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg.payload[0]; + + memcpy(p->key, key, sizeof(p->key)); // char[32]:key + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; + msg.msgid = MAVLINK_MSG_ID_AUTH_KEY; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) +{ + mavlink_header_t hdr; + mavlink_auth_key_t payload; + uint16_t checksum; + mavlink_auth_key_t *p = &payload; + + memcpy(p->key, key, sizeof(p->key)); // char[32]:key + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; + hdr.msgid = MAVLINK_MSG_ID_AUTH_KEY; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -85,11 +130,12 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* * * @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) { + mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*32); - return sizeof(char)*32; + memcpy(key, p->key, sizeof(p->key)); + return sizeof(p->key); } /** @@ -100,5 +146,5 @@ 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) { - mavlink_msg_auth_key_get_key(msg, auth_key->key); + memcpy( auth_key, msg->payload, sizeof(mavlink_auth_key_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 69d8ed64a..00e13e291 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -1,6 +1,8 @@ // MESSAGE BOOT PACKING #define MAVLINK_MSG_ID_BOOT 1 +#define MAVLINK_MSG_ID_BOOT_LEN 4 +#define MAVLINK_MSG_1_LEN 4 typedef struct __mavlink_boot_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_boot_t } mavlink_boot_t; - - /** * @brief Pack a boot message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_boot_t */ 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; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version + p->version = version; // uint32_t:The onboard software version - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BOOT_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen */ 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; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - i += put_uint32_t_by_index(version, i, msg->payload); // The onboard software version + p->version = version; // uint32_t: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, MAVLINK_MSG_ID_BOOT_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon * @param version The onboard software version */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { mavlink_message_t msg; - mavlink_msg_boot_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, version); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_boot_t *p = (mavlink_boot_t *)&msg.payload[0]; + + p->version = version; // uint32_t:The onboard software version + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_BOOT_LEN; + msg.msgid = MAVLINK_MSG_ID_BOOT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) +{ + mavlink_header_t hdr; + mavlink_boot_t payload; + uint16_t checksum; + mavlink_boot_t *p = &payload; + + p->version = version; // uint32_t:The onboard software version + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_BOOT_LEN; + hdr.msgid = MAVLINK_MSG_ID_BOOT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,12 +131,8 @@ 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; + mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; + return (uint32_t)(p->version); } /** @@ -102,5 +143,5 @@ 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) { - boot->version = mavlink_msg_boot_get_version(msg); + memcpy( boot, msg->payload, sizeof(mavlink_boot_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h index 3eab478cb..fdc783cac 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -1,6 +1,8 @@ // MESSAGE CHANGE_OPERATOR_CONTROL PACKING #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 +#define MAVLINK_MSG_5_LEN 28 typedef struct __mavlink_change_operator_control_t { @@ -10,10 +12,8 @@ typedef struct __mavlink_change_operator_control_t 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_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 - /** * @brief Pack a change_operator_control message * @param system_id ID of this system @@ -28,15 +28,15 @@ typedef struct __mavlink_change_operator_control_t */ 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; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; 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 "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[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 "!?,.-" - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } /** @@ -53,15 +53,15 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i */ 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; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; 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 "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[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 "!?,.-" - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { 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); + uint16_t checksum; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[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 "!?,.-" + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_change_operator_control_t payload; + uint16_t checksum; + mavlink_change_operator_control_t *p = &payload; + + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t: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. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[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 "!?,.-" + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ 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]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +167,8 @@ 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]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->control_request); } /** @@ -125,7 +178,8 @@ 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]; + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; + return (uint8_t)(p->version); } /** @@ -133,11 +187,12 @@ 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) { + mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t), sizeof(char)*25); - return sizeof(char)*25; + memcpy(passkey, p->passkey, sizeof(p->passkey)); + return sizeof(p->passkey); } /** @@ -148,8 +203,5 @@ 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) { - 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); + memcpy( change_operator_control, msg->payload, sizeof(mavlink_change_operator_control_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h index 3529ae059..b49883dd2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h @@ -1,6 +1,8 @@ // MESSAGE CHANGE_OPERATOR_CONTROL_ACK PACKING #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 +#define MAVLINK_MSG_6_LEN 3 typedef struct __mavlink_change_operator_control_ack_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_change_operator_control_ack_t } mavlink_change_operator_control_ack_t; - - /** * @brief Pack a change_operator_control_ack message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_change_operator_control_ack_t */ 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; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; 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 + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t: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, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst */ 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; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; 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 + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t: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, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { 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); + uint16_t checksum; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg.payload[0]; + + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_change_operator_control_ack_t payload; + uint16_t checksum; + mavlink_change_operator_control_ack_t *p = &payload; + + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ 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]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->gcs_system_id); } /** @@ -108,7 +158,8 @@ 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]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->control_request); } /** @@ -118,7 +169,8 @@ 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]; + mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; + return (uint8_t)(p->ack); } /** @@ -129,7 +181,5 @@ 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) { - 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); + memcpy( change_operator_control_ack, msg->payload, sizeof(mavlink_change_operator_control_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index dbc123570..c22eb7983 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -1,6 +1,8 @@ // MESSAGE COMMAND PACKING #define MAVLINK_MSG_ID_COMMAND 75 +#define MAVLINK_MSG_ID_COMMAND_LEN 20 +#define MAVLINK_MSG_75_LEN 20 typedef struct __mavlink_command_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_command_t } mavlink_command_t; - - /** * @brief Pack a command message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_command_t */ 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; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; 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. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float: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, MAVLINK_MSG_ID_COMMAND_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t compo */ 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; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; 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. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float: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, MAVLINK_MSG_ID_COMMAND_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param4 Parameter 4, as defined by MAV_CMD enum. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { 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); + uint16_t checksum; + mavlink_command_t *p = (mavlink_command_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_COMMAND_LEN; + msg.msgid = MAVLINK_MSG_ID_COMMAND; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_command_t payload; + uint16_t checksum; + mavlink_command_t *p = &payload; + + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_COMMAND_LEN; + hdr.msgid = MAVLINK_MSG_ID_COMMAND; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,7 +187,8 @@ 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]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -138,7 +198,8 @@ 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]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -148,7 +209,8 @@ 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]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->command); } /** @@ -158,7 +220,8 @@ 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]; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (uint8_t)(p->confirmation); } /** @@ -168,12 +231,8 @@ 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; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param1); } /** @@ -183,12 +242,8 @@ 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; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param2); } /** @@ -198,12 +253,8 @@ 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; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param3); } /** @@ -213,12 +264,8 @@ 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; + mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; + return (float)(p->param4); } /** @@ -229,12 +276,5 @@ 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) { - command->target_system = mavlink_msg_command_get_target_system(msg); - command->target_component = mavlink_msg_command_get_target_component(msg); - command->command = mavlink_msg_command_get_command(msg); - command->confirmation = mavlink_msg_command_get_confirmation(msg); - command->param1 = mavlink_msg_command_get_param1(msg); - command->param2 = mavlink_msg_command_get_param2(msg); - command->param3 = mavlink_msg_command_get_param3(msg); - command->param4 = mavlink_msg_command_get_param4(msg); + memcpy( command, msg->payload, sizeof(mavlink_command_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index 32b39da3a..a82703d53 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -1,6 +1,8 @@ // MESSAGE COMMAND_ACK PACKING #define MAVLINK_MSG_ID_COMMAND_ACK 76 +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 +#define MAVLINK_MSG_76_LEN 8 typedef struct __mavlink_command_ack_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_command_ack_t } mavlink_command_ack_t; - - /** * @brief Pack a command_ack message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_command_ack_t */ 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; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; 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 + p->command = command; // float:Current airspeed in m/s + p->result = result; // float: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, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c */ 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; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; 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 + p->command = command; // float:Current airspeed in m/s + p->result = result; // float: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, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { 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); + uint16_t checksum; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg.payload[0]; + + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_COMMAND_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) +{ + mavlink_header_t hdr; + mavlink_command_ack_t payload; + uint16_t checksum; + mavlink_command_ack_t *p = &payload; + + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_COMMAND_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,12 +139,8 @@ 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; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; + return (float)(p->command); } /** @@ -107,12 +150,8 @@ 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; + mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; + return (float)(p->result); } /** @@ -123,6 +162,5 @@ 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) { - command_ack->command = mavlink_msg_command_ack_get_command(msg); - command_ack->result = mavlink_msg_command_ack_get_result(msg); + memcpy( command_ack, msg->payload, sizeof(mavlink_command_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 746e889aa..00749b618 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -1,6 +1,8 @@ // MESSAGE CONTROL_STATUS PACKING #define MAVLINK_MSG_ID_CONTROL_STATUS 52 +#define MAVLINK_MSG_ID_CONTROL_STATUS_LEN 8 +#define MAVLINK_MSG_52_LEN 8 typedef struct __mavlink_control_status_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_control_status_t } mavlink_control_status_t; - - /** * @brief Pack a control_status message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_control_status_t */ 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; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t: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, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ */ 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; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - i += put_uint8_t_by_index(position_fix, i, msg->payload); // Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(vision_fix, i, msg->payload); // Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - i += put_uint8_t_by_index(gps_fix, i, msg->payload); // GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - i += put_uint8_t_by_index(ahrs_health, i, msg->payload); // Attitude estimation health: 0: poor, 255: excellent - i += put_uint8_t_by_index(control_att, i, msg->payload); // 0: Attitude control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_xy, i, msg->payload); // 0: X, Y position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_z, i, msg->payload); // 0: Z position control disabled, 1: enabled - i += put_uint8_t_by_index(control_pos_yaw, i, msg->payload); // 0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t: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, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { mavlink_message_t msg; - mavlink_msg_control_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg.payload[0]; + + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_control_status_t payload; + uint16_t checksum; + mavlink_control_status_t *p = &payload; + + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,7 +187,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->position_fix); } /** @@ -138,7 +198,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->vision_fix); } /** @@ -148,7 +209,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->gps_fix); } /** @@ -158,7 +220,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->ahrs_health); } /** @@ -168,7 +231,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_att); } /** @@ -178,7 +242,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_xy); } /** @@ -188,7 +253,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_z); } /** @@ -198,7 +264,8 @@ 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]; + mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; + return (uint8_t)(p->control_pos_yaw); } /** @@ -209,12 +276,5 @@ 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) { - control_status->position_fix = mavlink_msg_control_status_get_position_fix(msg); - control_status->vision_fix = mavlink_msg_control_status_get_vision_fix(msg); - control_status->gps_fix = mavlink_msg_control_status_get_gps_fix(msg); - control_status->ahrs_health = mavlink_msg_control_status_get_ahrs_health(msg); - control_status->control_att = mavlink_msg_control_status_get_control_att(msg); - control_status->control_pos_xy = mavlink_msg_control_status_get_control_pos_xy(msg); - control_status->control_pos_z = mavlink_msg_control_status_get_control_pos_z(msg); - control_status->control_pos_yaw = mavlink_msg_control_status_get_control_pos_yaw(msg); + memcpy( control_status, msg->payload, sizeof(mavlink_control_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index e98be847a..992406335 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -1,6 +1,8 @@ // MESSAGE DEBUG PACKING #define MAVLINK_MSG_ID_DEBUG 255 +#define MAVLINK_MSG_ID_DEBUG_LEN 5 +#define MAVLINK_MSG_255_LEN 5 typedef struct __mavlink_debug_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_debug_t } mavlink_debug_t; - - /** * @brief Pack a debug message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_debug_t */ 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; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; 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 + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone */ 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; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; 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 + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float: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, MAVLINK_MSG_ID_DEBUG_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo * @param value DEBUG value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { mavlink_message_t msg; - mavlink_msg_debug_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, ind, value); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_debug_t *p = (mavlink_debug_t *)&msg.payload[0]; + + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DEBUG_LEN; + msg.msgid = MAVLINK_MSG_ID_DEBUG; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) +{ + mavlink_header_t hdr; + mavlink_debug_t payload; + uint16_t checksum; + mavlink_debug_t *p = &payload; + + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DEBUG_LEN; + hdr.msgid = MAVLINK_MSG_ID_DEBUG; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; + return (uint8_t)(p->ind); } /** @@ -102,12 +150,8 @@ 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; + mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; + return (float)(p->value); } /** @@ -118,6 +162,5 @@ 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) { - debug->ind = mavlink_msg_debug_get_ind(msg); - debug->value = mavlink_msg_debug_get_value(msg); + memcpy( debug, msg->payload, sizeof(mavlink_debug_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 6ea9e0558..b31289ac8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -1,6 +1,8 @@ // MESSAGE DEBUG_VECT PACKING #define MAVLINK_MSG_ID_DEBUG_VECT 251 +#define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 +#define MAVLINK_MSG_251_LEN 30 typedef struct __mavlink_debug_vect_t { @@ -11,10 +13,8 @@ typedef struct __mavlink_debug_vect_t float z; ///< z } mavlink_debug_vect_t; - #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 - /** * @brief Pack a debug_vect message * @param system_id ID of this system @@ -30,16 +30,16 @@ typedef struct __mavlink_debug_vect_t */ 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; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } /** @@ -57,16 +57,16 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co */ 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; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } /** @@ -93,12 +93,65 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param z z */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) { 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); + uint16_t checksum; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg.payload[0]; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; + msg.msgid = MAVLINK_MSG_ID_DEBUG_VECT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_debug_vect_t payload; + uint16_t checksum; + mavlink_debug_vect_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; + hdr.msgid = MAVLINK_MSG_ID_DEBUG_VECT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -109,11 +162,12 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha * * @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) { + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -123,16 +177,8 @@ 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; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -142,12 +188,8 @@ 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; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -157,12 +199,8 @@ 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; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -172,12 +210,8 @@ 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; + mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -188,9 +222,5 @@ 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) { - 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); + memcpy( debug_vect, msg->payload, sizeof(mavlink_debug_vect_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index 9d27f7714..bb5112eba 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -1,6 +1,8 @@ // MESSAGE GLOBAL_POSITION PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION 33 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 +#define MAVLINK_MSG_33_LEN 32 typedef struct __mavlink_global_position_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_global_position_t } mavlink_global_position_t; - - /** * @brief Pack a global_position message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_global_position_t */ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float: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, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 */ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float: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, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin * @param vz Z Speed (in Altitude direction, positive: going up) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { 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); + uint16_t checksum; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; + msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_global_position_t payload; + uint16_t checksum; + mavlink_global_position_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; + hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -156,12 +201,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -171,12 +212,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -186,12 +223,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vx); } /** @@ -201,12 +234,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vy); } /** @@ -216,12 +245,8 @@ 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; + mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; + return (float)(p->vz); } /** @@ -232,11 +257,5 @@ 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) { - global_position->usec = mavlink_msg_global_position_get_usec(msg); - global_position->lat = mavlink_msg_global_position_get_lat(msg); - global_position->lon = mavlink_msg_global_position_get_lon(msg); - global_position->alt = mavlink_msg_global_position_get_alt(msg); - global_position->vx = mavlink_msg_global_position_get_vx(msg); - global_position->vy = mavlink_msg_global_position_get_vy(msg); - global_position->vz = mavlink_msg_global_position_get_vz(msg); + memcpy( global_position, msg->payload, sizeof(mavlink_global_position_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h index 1b3277a35..fb11a417e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -1,6 +1,8 @@ // MESSAGE GLOBAL_POSITION_INT PACKING #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 18 +#define MAVLINK_MSG_73_LEN 18 typedef struct __mavlink_global_position_int_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_global_position_int_t } mavlink_global_position_int_t; - - /** * @brief Pack a global_position_int message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_global_position_int_t */ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; 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 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t: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, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u */ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; 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 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t: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, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) { 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); + uint16_t checksum; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg.payload[0]; + + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; + msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_global_position_int_t payload; + uint16_t checksum; + mavlink_global_position_int_t *p = &payload; + + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->lat); } /** @@ -131,12 +182,8 @@ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->lon); } /** @@ -146,12 +193,8 @@ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int32_t)(p->alt); } /** @@ -161,10 +204,8 @@ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vx); } /** @@ -174,10 +215,8 @@ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vy); } /** @@ -187,10 +226,8 @@ 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; + mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; + return (int16_t)(p->vz); } /** @@ -201,10 +238,5 @@ 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) { - 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); + memcpy( global_position_int, msg->payload, sizeof(mavlink_global_position_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h index 156252854..8834ee6ce 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h @@ -1,6 +1,8 @@ // MESSAGE GPS_LOCAL_ORIGIN_SET PACKING #define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET 49 +#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN 12 +#define MAVLINK_MSG_49_LEN 12 typedef struct __mavlink_gps_local_origin_set_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_gps_local_origin_set_t } mavlink_gps_local_origin_set_t; - - /** * @brief Pack a gps_local_origin_set message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_gps_local_origin_set_t */ 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; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; 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 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, */ 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; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; 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 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t: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, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id * @param altitude Altitude(WGS84), expressed as * 1000 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { 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); + uint16_t checksum; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg.payload[0]; + + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +{ + mavlink_header_t hdr; + mavlink_gps_local_origin_set_t payload; + uint16_t checksum; + mavlink_gps_local_origin_set_t *p = &payload; + + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,12 +147,8 @@ 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; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->latitude); } /** @@ -113,12 +158,8 @@ 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; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->longitude); } /** @@ -128,12 +169,8 @@ 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; + mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; + return (int32_t)(p->altitude); } /** @@ -144,7 +181,5 @@ 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) { - 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); + memcpy( gps_local_origin_set, msg->payload, sizeof(mavlink_gps_local_origin_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 52d33a23b..ecaeb8b84 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -1,6 +1,8 @@ // MESSAGE GPS_RAW PACKING #define MAVLINK_MSG_ID_GPS_RAW 32 +#define MAVLINK_MSG_ID_GPS_RAW_LEN 37 +#define MAVLINK_MSG_32_LEN 37 typedef struct __mavlink_gps_raw_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_gps_raw_t } mavlink_gps_raw_t; - - /** * @brief Pack a gps_raw message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_gps_raw_t */ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float: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, MAVLINK_MSG_ID_GPS_RAW_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo */ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float: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, MAVLINK_MSG_ID_GPS_RAW_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @param hdg Compass heading in degrees, 0..360 degrees */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) { 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); + uint16_t checksum; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_RAW_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_RAW; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_gps_raw_t payload; + uint16_t checksum; + mavlink_gps_raw_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,16 +195,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -153,7 +206,8 @@ 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]; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (uint8_t)(p->fix_type); } /** @@ -163,12 +217,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -178,12 +228,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -193,12 +239,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -208,12 +250,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->eph); } /** @@ -223,12 +261,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->epv); } /** @@ -238,12 +272,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->v); } /** @@ -253,12 +283,8 @@ 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; + mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; + return (float)(p->hdg); } /** @@ -269,13 +295,5 @@ 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) { - gps_raw->usec = mavlink_msg_gps_raw_get_usec(msg); - gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); - gps_raw->lat = mavlink_msg_gps_raw_get_lat(msg); - gps_raw->lon = mavlink_msg_gps_raw_get_lon(msg); - gps_raw->alt = mavlink_msg_gps_raw_get_alt(msg); - gps_raw->eph = mavlink_msg_gps_raw_get_eph(msg); - gps_raw->epv = mavlink_msg_gps_raw_get_epv(msg); - gps_raw->v = mavlink_msg_gps_raw_get_v(msg); - gps_raw->hdg = mavlink_msg_gps_raw_get_hdg(msg); + memcpy( gps_raw, msg->payload, sizeof(mavlink_gps_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h index b49498feb..9fc5360b1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -1,6 +1,8 @@ // MESSAGE GPS_RAW_INT PACKING #define MAVLINK_MSG_ID_GPS_RAW_INT 25 +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 37 +#define MAVLINK_MSG_25_LEN 37 typedef struct __mavlink_gps_raw_int_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_gps_raw_int_t } mavlink_gps_raw_int_t; - - /** * @brief Pack a gps_raw_int message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_gps_raw_int_t */ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float: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, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c */ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float: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, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param hdg Compass heading in degrees, 0..360 degrees */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) { 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); + uint16_t checksum; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_gps_raw_int_t payload; + uint16_t checksum; + mavlink_gps_raw_int_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t: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. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed (m/s) + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,16 +195,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -153,7 +206,8 @@ 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]; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (uint8_t)(p->fix_type); } /** @@ -163,12 +217,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->lat); } /** @@ -178,12 +228,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->lon); } /** @@ -193,12 +239,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (int32_t)(p->alt); } /** @@ -208,12 +250,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->eph); } /** @@ -223,12 +261,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->epv); } /** @@ -238,12 +272,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->v); } /** @@ -253,12 +283,8 @@ 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; + mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; + return (float)(p->hdg); } /** @@ -269,13 +295,5 @@ 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) { - gps_raw_int->usec = mavlink_msg_gps_raw_int_get_usec(msg); - gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); - gps_raw_int->lat = mavlink_msg_gps_raw_int_get_lat(msg); - gps_raw_int->lon = mavlink_msg_gps_raw_int_get_lon(msg); - gps_raw_int->alt = mavlink_msg_gps_raw_int_get_alt(msg); - gps_raw_int->eph = mavlink_msg_gps_raw_int_get_eph(msg); - gps_raw_int->epv = mavlink_msg_gps_raw_int_get_epv(msg); - gps_raw_int->v = mavlink_msg_gps_raw_int_get_v(msg); - gps_raw_int->hdg = mavlink_msg_gps_raw_int_get_hdg(msg); + memcpy( gps_raw_int, msg->payload, sizeof(mavlink_gps_raw_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h index 47ddc2e23..f7aaf96da 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h @@ -1,6 +1,8 @@ // MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING #define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 +#define MAVLINK_MSG_48_LEN 14 typedef struct __mavlink_gps_set_global_origin_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_gps_set_global_origin_t } mavlink_gps_set_global_origin_t; - - /** * @brief Pack a gps_set_global_origin message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_gps_set_global_origin_t */ 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; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, */ 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; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t: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, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i * @param altitude global position * 1000 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { 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); + uint16_t checksum; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_gps_set_global_origin_t payload; + uint16_t checksum; + mavlink_gps_set_global_origin_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ 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]; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -120,7 +174,8 @@ 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]; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -130,12 +185,8 @@ 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; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->latitude); } /** @@ -145,12 +196,8 @@ 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; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->longitude); } /** @@ -160,12 +207,8 @@ 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; + mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; + return (int32_t)(p->altitude); } /** @@ -176,9 +219,5 @@ 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) { - 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); + memcpy( gps_set_global_origin, msg->payload, sizeof(mavlink_gps_set_global_origin_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index fd04a9a26..484de2c01 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -1,25 +1,25 @@ // MESSAGE GPS_STATUS PACKING #define MAVLINK_MSG_ID_GPS_STATUS 27 +#define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 +#define MAVLINK_MSG_27_LEN 101 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 satellite_prn[20]; ///< Global satellite ID + uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization + uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite } mavlink_gps_status_t; - #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 - /** * @brief Pack a gps_status message * @param system_id ID of this system @@ -34,19 +34,19 @@ 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 uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - uint16_t i = 0; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; 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 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]: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, MAVLINK_MSG_ID_GPS_STATUS_LEN); } /** @@ -63,19 +63,19 @@ 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 uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { - uint16_t i = 0; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; 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 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]: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, MAVLINK_MSG_ID_GPS_STATUS_LEN); } /** @@ -103,12 +103,67 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_snr Signal to noise ratio of satellite */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const int8_t* satellite_prn, const int8_t* satellite_used, const int8_t* satellite_elevation, const int8_t* satellite_azimuth, const int8_t* satellite_snr) +static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_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); + uint16_t checksum; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg.payload[0]; + + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) +{ + mavlink_header_t hdr; + mavlink_gps_status_t payload; + uint16_t checksum; + mavlink_gps_status_t *p = &payload; + + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -121,7 +176,8 @@ 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]; + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; + return (uint8_t)(p->satellites_visible); } /** @@ -129,11 +185,12 @@ 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, uint8_t* satellite_prn) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t), 20); - return 20; + memcpy(satellite_prn, p->satellite_prn, sizeof(p->satellite_prn)); + return sizeof(p->satellite_prn); } /** @@ -141,11 +198,12 @@ 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, uint8_t* satellite_used) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20, 20); - return 20; + memcpy(satellite_used, p->satellite_used, sizeof(p->satellite_used)); + return sizeof(p->satellite_used); } /** @@ -153,11 +211,12 @@ 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, uint8_t* satellite_elevation) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20, 20); - return 20; + memcpy(satellite_elevation, p->satellite_elevation, sizeof(p->satellite_elevation)); + return sizeof(p->satellite_elevation); } /** @@ -165,11 +224,12 @@ 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, uint8_t* satellite_azimuth) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20, 20); - return 20; + memcpy(satellite_azimuth, p->satellite_azimuth, sizeof(p->satellite_azimuth)); + return sizeof(p->satellite_azimuth); } /** @@ -177,11 +237,12 @@ 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, uint8_t* satellite_snr) { + mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+20+20+20+20, 20); - return 20; + memcpy(satellite_snr, p->satellite_snr, sizeof(p->satellite_snr)); + return sizeof(p->satellite_snr); } /** @@ -192,10 +253,5 @@ 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) { - 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); + memcpy( gps_status, msg->payload, sizeof(mavlink_gps_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 0e5c4db5c..fea5381e9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_0_LEN 3 typedef struct __mavlink_heartbeat_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_heartbeat_t } mavlink_heartbeat_t; - - /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -24,14 +24,14 @@ typedef struct __mavlink_heartbeat_t */ 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; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; 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 + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message(msg, system_id, component_id, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -46,14 +46,14 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com */ 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; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; 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 + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -77,12 +77,61 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) +{ + mavlink_header_t hdr; + mavlink_heartbeat_t payload; + uint16_t checksum; + mavlink_heartbeat_t *p = &payload; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -95,7 +144,8 @@ 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]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -105,7 +155,8 @@ 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]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->autopilot); } /** @@ -115,7 +166,8 @@ 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]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->mavlink_version); } /** @@ -126,7 +178,5 @@ 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) { - 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); + memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index c9cb453c0..2fbe01f2a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION 31 +#define MAVLINK_MSG_ID_LOCAL_POSITION_LEN 32 +#define MAVLINK_MSG_31_LEN 32 typedef struct __mavlink_local_position_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_local_position_t } mavlink_local_position_t; - - /** * @brief Pack a local_position message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_local_position_t */ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ */ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; 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 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float: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, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint * @param vz Z Speed */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { 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); + uint16_t checksum; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; + msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_local_position_t payload; + uint16_t checksum; + mavlink_local_position_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +201,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +212,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +223,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vx); } /** @@ -201,12 +234,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vy); } /** @@ -216,12 +245,8 @@ 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; + mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; + return (float)(p->vz); } /** @@ -232,11 +257,5 @@ 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) { - local_position->usec = mavlink_msg_local_position_get_usec(msg); - local_position->x = mavlink_msg_local_position_get_x(msg); - local_position->y = mavlink_msg_local_position_get_y(msg); - local_position->z = mavlink_msg_local_position_get_z(msg); - local_position->vx = mavlink_msg_local_position_get_vx(msg); - local_position->vy = mavlink_msg_local_position_get_vy(msg); - local_position->vz = mavlink_msg_local_position_get_vz(msg); + memcpy( local_position, msg->payload, sizeof(mavlink_local_position_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h index fcf473347..2f6ceb808 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION_SETPOINT PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 16 +#define MAVLINK_MSG_51_LEN 16 typedef struct __mavlink_local_position_setpoint_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_local_position_setpoint_t } mavlink_local_position_setpoint_t; - - /** * @brief Pack a local_position_setpoint message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_local_position_setpoint_t */ 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; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i */ 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; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system * @param yaw Desired yaw angle */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { 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); + uint16_t checksum; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg.payload[0]; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; + msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) +{ + mavlink_header_t hdr; + mavlink_local_position_setpoint_t payload; + uint16_t checksum; + mavlink_local_position_setpoint_t *p = &payload; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +155,8 @@ 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; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -119,12 +166,8 @@ 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; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -134,12 +177,8 @@ 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; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -149,12 +188,8 @@ 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; + mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -165,8 +200,5 @@ 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) { - 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); + memcpy( local_position_setpoint, msg->payload, sizeof(mavlink_local_position_setpoint_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h index f3383287e..9f34862b8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h @@ -1,6 +1,8 @@ // MESSAGE LOCAL_POSITION_SETPOINT_SET PACKING #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN 18 +#define MAVLINK_MSG_50_LEN 18 typedef struct __mavlink_local_position_setpoint_set_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_local_position_setpoint_set_t } mavlink_local_position_setpoint_set_t; - - /** * @brief Pack a local_position_setpoint_set message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_local_position_setpoint_set_t */ 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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst */ 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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy * @param yaw Desired yaw angle */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { 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); + uint16_t checksum; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_local_position_setpoint_set_t payload; + uint16_t checksum; + mavlink_local_position_setpoint_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +171,8 @@ 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]; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -126,7 +182,8 @@ 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]; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -136,12 +193,8 @@ 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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -151,12 +204,8 @@ 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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -166,12 +215,8 @@ 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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -181,12 +226,8 @@ 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; + mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -197,10 +238,5 @@ 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) { - 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); + memcpy( local_position_setpoint_set, msg->payload, sizeof(mavlink_local_position_setpoint_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index deec098f6..d3aede95b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -1,6 +1,8 @@ // MESSAGE MANUAL_CONTROL PACKING #define MAVLINK_MSG_ID_MANUAL_CONTROL 69 +#define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 21 +#define MAVLINK_MSG_69_LEN 21 typedef struct __mavlink_manual_control_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_manual_control_t } mavlink_manual_control_t; - - /** * @brief Pack a manual_control message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_manual_control_t */ 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; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ */ 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; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t: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, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint * @param thrust_manual thrust auto:0, manual:1 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { 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); + uint16_t checksum; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_manual_control_t payload; + uint16_t checksum; + mavlink_manual_control_t *p = &payload; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +195,8 @@ 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]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -144,12 +206,8 @@ 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; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -159,12 +217,8 @@ 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; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -174,12 +228,8 @@ 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; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -189,12 +239,8 @@ 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; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (float)(p->thrust); } /** @@ -204,7 +250,8 @@ 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]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->roll_manual); } /** @@ -214,7 +261,8 @@ 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]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->pitch_manual); } /** @@ -224,7 +272,8 @@ 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]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->yaw_manual); } /** @@ -234,7 +283,8 @@ 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]; + mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; + return (uint8_t)(p->thrust_manual); } /** @@ -245,13 +295,5 @@ 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) { - manual_control->target = mavlink_msg_manual_control_get_target(msg); - manual_control->roll = mavlink_msg_manual_control_get_roll(msg); - manual_control->pitch = mavlink_msg_manual_control_get_pitch(msg); - manual_control->yaw = mavlink_msg_manual_control_get_yaw(msg); - manual_control->thrust = mavlink_msg_manual_control_get_thrust(msg); - manual_control->roll_manual = mavlink_msg_manual_control_get_roll_manual(msg); - manual_control->pitch_manual = mavlink_msg_manual_control_get_pitch_manual(msg); - manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg); - manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg); + memcpy( manual_control, msg->payload, sizeof(mavlink_manual_control_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h index aaff215ce..6f4fb8830 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -1,6 +1,8 @@ // MESSAGE NAMED_VALUE_FLOAT PACKING #define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 252 +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 14 +#define MAVLINK_MSG_252_LEN 14 typedef struct __mavlink_named_value_float_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_named_value_float_t float value; ///< Floating point value } mavlink_named_value_float_t; - #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 - /** * @brief Pack a named_value_float message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_named_value_float_t */ 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; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin */ 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; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float: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, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param value Floating point value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) { 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); + uint16_t checksum; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg.payload[0]; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; + msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) +{ + mavlink_header_t hdr; + mavlink_named_value_float_t payload; + uint16_t checksum; + mavlink_named_value_float_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -91,11 +138,12 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, co * * @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) { + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -105,12 +153,8 @@ 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; + mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; + return (float)(p->value); } /** @@ -121,6 +165,5 @@ 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) { - mavlink_msg_named_value_float_get_name(msg, named_value_float->name); - named_value_float->value = mavlink_msg_named_value_float_get_value(msg); + memcpy( named_value_float, msg->payload, sizeof(mavlink_named_value_float_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h index 92bf5fcb2..a793614f5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -1,6 +1,8 @@ // MESSAGE NAMED_VALUE_INT PACKING #define MAVLINK_MSG_ID_NAMED_VALUE_INT 253 +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 14 +#define MAVLINK_MSG_253_LEN 14 typedef struct __mavlink_named_value_int_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_named_value_int_t int32_t value; ///< Signed integer value } mavlink_named_value_int_t; - #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 - /** * @brief Pack a named_value_int message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_named_value_int_t */ 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; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 */ 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; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; 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 + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t: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, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param value Signed integer value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) { 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); + uint16_t checksum; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg.payload[0]; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; + msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) +{ + mavlink_header_t hdr; + mavlink_named_value_int_t payload; + uint16_t checksum; + mavlink_named_value_int_t *p = &payload; + + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -91,11 +138,12 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, cons * * @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) { + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(char)*10); - return sizeof(char)*10; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -105,12 +153,8 @@ 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; + mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; + return (int32_t)(p->value); } /** @@ -121,6 +165,5 @@ 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) { - mavlink_msg_named_value_int_get_name(msg, named_value_int->name); - named_value_int->value = mavlink_msg_named_value_int_get_value(msg); + memcpy( named_value_int, msg->payload, sizeof(mavlink_named_value_int_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h index 8d0b81eb8..f19d85b0a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE NAV_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 +#define MAVLINK_MSG_62_LEN 26 typedef struct __mavlink_nav_controller_output_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_nav_controller_output_t } mavlink_nav_controller_output_t; - - /** * @brief Pack a nav_controller_output message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_nav_controller_output_t */ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float: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, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, */ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - i += put_float_by_index(nav_roll, i, msg->payload); // Current desired roll in degrees - i += put_float_by_index(nav_pitch, i, msg->payload); // Current desired pitch in degrees - i += put_int16_t_by_index(nav_bearing, i, msg->payload); // Current desired heading in degrees - i += put_int16_t_by_index(target_bearing, i, msg->payload); // Bearing to current waypoint/target in degrees - i += put_uint16_t_by_index(wp_dist, i, msg->payload); // Distance to active waypoint in meters - i += put_float_by_index(alt_error, i, msg->payload); // Current altitude error in meters - i += put_float_by_index(aspd_error, i, msg->payload); // Current airspeed error in meters/second - i += put_float_by_index(xtrack_error, i, msg->payload); // Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float: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, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i * @param xtrack_error Current crosstrack error on x-y plane in meters */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { 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); + uint16_t checksum; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg.payload[0]; + + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; + msg.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_nav_controller_output_t payload; + uint16_t checksum; + mavlink_nav_controller_output_t *p = &payload; + + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,12 +187,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->nav_roll); } /** @@ -143,12 +198,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->nav_pitch); } /** @@ -158,10 +209,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (int16_t)(p->nav_bearing); } /** @@ -171,10 +220,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (int16_t)(p->target_bearing); } /** @@ -184,10 +231,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (uint16_t)(p->wp_dist); } /** @@ -197,12 +242,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->alt_error); } /** @@ -212,12 +253,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->aspd_error); } /** @@ -227,12 +264,8 @@ 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; + mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; + return (float)(p->xtrack_error); } /** @@ -243,12 +276,5 @@ 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) { - nav_controller_output->nav_roll = mavlink_msg_nav_controller_output_get_nav_roll(msg); - nav_controller_output->nav_pitch = mavlink_msg_nav_controller_output_get_nav_pitch(msg); - nav_controller_output->nav_bearing = mavlink_msg_nav_controller_output_get_nav_bearing(msg); - nav_controller_output->target_bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg); - nav_controller_output->wp_dist = mavlink_msg_nav_controller_output_get_wp_dist(msg); - nav_controller_output->alt_error = mavlink_msg_nav_controller_output_get_alt_error(msg); - nav_controller_output->aspd_error = mavlink_msg_nav_controller_output_get_aspd_error(msg); - nav_controller_output->xtrack_error = mavlink_msg_nav_controller_output_get_xtrack_error(msg); + memcpy( nav_controller_output, msg->payload, sizeof(mavlink_nav_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h index e4912fb59..d53cfe7d8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -1,6 +1,8 @@ // MESSAGE PARAM_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_21_LEN 2 typedef struct __mavlink_param_request_list_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_param_request_list_t } mavlink_param_request_list_t; - - /** * @brief Pack a param_request_list message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_param_request_list_t */ 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; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui */ 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; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t: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, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, * @param target_component Component ID */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { 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); + uint16_t checksum; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ + mavlink_header_t hdr; + mavlink_param_request_list_t payload; + uint16_t checksum; + mavlink_param_request_list_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +150,8 @@ 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]; + mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +162,5 @@ 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) { - 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); + memcpy( param_request_list, msg->payload, sizeof(mavlink_param_request_list_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h index 2cd9d2932..a60179da9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -1,19 +1,19 @@ // MESSAGE PARAM_REQUEST_READ PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 19 +#define MAVLINK_MSG_20_LEN 19 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 + char 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_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15 - /** * @brief Pack a param_request_read message * @param system_id ID of this system @@ -26,17 +26,17 @@ 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 char* param_id, int16_t param_index) { - uint16_t i = 0; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t: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, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } /** @@ -51,17 +51,17 @@ 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 char* param_id, int16_t param_index) { - uint16_t i = 0; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t: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, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t* param_id, int16_t param_index) +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* 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); + uint16_t checksum; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) +{ + mavlink_header_t hdr; + mavlink_param_request_read_t payload; + uint16_t checksum; + mavlink_param_request_read_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ 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]; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +167,8 @@ 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]; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -123,11 +176,12 @@ 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, char* param_id) { + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -137,10 +191,8 @@ 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; + mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; + return (int16_t)(p->param_index); } /** @@ -151,8 +203,5 @@ 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) { - 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); + memcpy( param_request_read, msg->payload, sizeof(mavlink_param_request_read_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index de1c31402..476c98095 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -1,19 +1,19 @@ // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 +#define MAVLINK_MSG_ID_PARAM_SET_LEN 21 +#define MAVLINK_MSG_23_LEN 21 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 + char param_id[15]; ///< Onboard parameter id float param_value; ///< Onboard parameter value } mavlink_param_set_t; - #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15 - /** * @brief Pack a param_set message * @param system_id ID of this system @@ -26,17 +26,17 @@ 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 char* param_id, float param_value) { - uint16_t i = 0; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); } /** @@ -51,17 +51,17 @@ 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 char* param_id, float param_value) { - uint16_t i = 0; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float: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, MAVLINK_MSG_ID_PARAM_SET_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_value Onboard parameter value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const int8_t* param_id, float param_value) +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* 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); + uint16_t checksum; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +{ + mavlink_header_t hdr; + mavlink_param_set_t payload; + uint16_t checksum; + mavlink_param_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ 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]; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -115,7 +167,8 @@ 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]; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -123,11 +176,12 @@ 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, char* param_id) { + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t), 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -137,12 +191,8 @@ 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; + mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; + return (float)(p->param_value); } /** @@ -153,8 +203,5 @@ 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) { - 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); + memcpy( param_set, msg->payload, sizeof(mavlink_param_set_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 239c96c42..383440845 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -1,19 +1,19 @@ // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 23 +#define MAVLINK_MSG_22_LEN 23 typedef struct __mavlink_param_value_t { - int8_t param_id[15]; ///< Onboard parameter id + char 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_PARAM_VALUE_FIELD_PARAM_ID_LEN 15 - /** * @brief Pack a param_value message * @param system_id ID of this system @@ -26,17 +26,17 @@ 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 char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - uint16_t i = 0; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; 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 + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } /** @@ -51,17 +51,17 @@ 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 char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { - uint16_t i = 0; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; 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 + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t: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, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_index Index of this onboard parameter */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const int8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* 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); + uint16_t checksum; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg.payload[0]; + + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; + msg.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) +{ + mavlink_header_t hdr; + mavlink_param_value_t payload; + uint16_t checksum; + mavlink_param_value_t *p = &payload; + + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; + hdr.msgid = MAVLINK_MSG_ID_PARAM_VALUE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -103,11 +154,12 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const in * * @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, char* param_id) { + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, 15); - return 15; + memcpy(param_id, p->param_id, sizeof(p->param_id)); + return sizeof(p->param_id); } /** @@ -117,12 +169,8 @@ 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; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (float)(p->param_value); } /** @@ -132,10 +180,8 @@ 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; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (uint16_t)(p->param_count); } /** @@ -145,10 +191,8 @@ 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; + mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; + return (uint16_t)(p->param_index); } /** @@ -159,8 +203,5 @@ 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) { - 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); + memcpy( param_value, msg->payload, sizeof(mavlink_param_value_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index 2a27b1e5c..2e89aa2b0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -1,6 +1,8 @@ // MESSAGE PING PACKING #define MAVLINK_MSG_ID_PING 3 +#define MAVLINK_MSG_ID_PING_LEN 14 +#define MAVLINK_MSG_3_LEN 14 typedef struct __mavlink_ping_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_ping_t } mavlink_ping_t; - - /** * @brief Pack a ping message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_ping_t */ 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; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; 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 + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t:Unix timestamp in microseconds - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen */ 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; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; 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 + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t: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, MAVLINK_MSG_ID_PING_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon * @param time Unix timestamp in microseconds */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { 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); + uint16_t checksum; + mavlink_ping_t *p = (mavlink_ping_t *)&msg.payload[0]; + + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t:Unix timestamp in microseconds + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PING_LEN; + msg.msgid = MAVLINK_MSG_ID_PING; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_ping_t payload; + uint16_t checksum; + mavlink_ping_t *p = &payload; + + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t: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 + p->target_component = target_component; // uint8_t: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 + p->time = time; // uint64_t:Unix timestamp in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PING_LEN; + hdr.msgid = MAVLINK_MSG_ID_PING; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +155,8 @@ 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; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint32_t)(p->seq); } /** @@ -119,7 +166,8 @@ 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]; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -129,7 +177,8 @@ 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]; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -139,16 +188,8 @@ 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; + mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; + return (uint64_t)(p->time); } /** @@ -159,8 +200,5 @@ 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) { - 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); + memcpy( ping, msg->payload, sizeof(mavlink_ping_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h index 389d9a238..2935301ef 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROLLER_OUTPUT PACKING #define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT 61 +#define MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN 5 +#define MAVLINK_MSG_61_LEN 5 typedef struct __mavlink_position_controller_output_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_position_controller_output_t } mavlink_position_controller_output_t; - - /** * @brief Pack a position_controller_output message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_position_controller_output_t */ static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { - uint16_t i = 0; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_position_controller_output_pack(uint8_t syste */ static inline uint16_t mavlink_msg_position_controller_output_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { - uint16_t i = 0; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - i += put_uint8_t_by_index(enabled, i, msg->payload); // 1: enabled, 0: disabled - i += put_int8_t_by_index(x, i, msg->payload); // Position x: -128: -100%, 127: +100% - i += put_int8_t_by_index(y, i, msg->payload); // Position y: -128: -100%, 127: +100% - i += put_int8_t_by_index(z, i, msg->payload); // Position z: -128: -100%, 127: +100% - i += put_int8_t_by_index(yaw, i, msg->payload); // Position yaw: -128: -100%, 127: +100% + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t sys * @param yaw Position yaw: -128: -100%, 127: +100% */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { mavlink_message_t msg; - mavlink_msg_position_controller_output_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg.payload[0]; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) +{ + mavlink_header_t hdr; + mavlink_position_controller_output_t payload; + uint16_t checksum; + mavlink_position_controller_output_t *p = &payload; + + p->enabled = enabled; // uint8_t:1: enabled, 0: disabled + p->x = x; // int8_t:Position x: -128: -100%, 127: +100% + p->y = y; // int8_t:Position y: -128: -100%, 127: +100% + p->z = z; // int8_t:Position z: -128: -100%, 127: +100% + p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (uint8_t)(p->enabled); } /** @@ -120,7 +174,8 @@ static inline uint8_t mavlink_msg_position_controller_output_get_enabled(const m */ static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->x); } /** @@ -130,7 +185,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_x(const mavlink_ */ static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->y); } /** @@ -140,7 +196,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_y(const mavlink_ */ static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->z); } /** @@ -150,7 +207,8 @@ static inline int8_t mavlink_msg_position_controller_output_get_z(const mavlink_ */ static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlink_message_t* msg) { - return (int8_t)(msg->payload+sizeof(uint8_t)+sizeof(int8_t)+sizeof(int8_t)+sizeof(int8_t))[0]; + mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg->payload[0]; + return (int8_t)(p->yaw); } /** @@ -161,9 +219,5 @@ static inline int8_t mavlink_msg_position_controller_output_get_yaw(const mavlin */ static inline void mavlink_msg_position_controller_output_decode(const mavlink_message_t* msg, mavlink_position_controller_output_t* position_controller_output) { - position_controller_output->enabled = mavlink_msg_position_controller_output_get_enabled(msg); - position_controller_output->x = mavlink_msg_position_controller_output_get_x(msg); - position_controller_output->y = mavlink_msg_position_controller_output_get_y(msg); - position_controller_output->z = mavlink_msg_position_controller_output_get_z(msg); - position_controller_output->yaw = mavlink_msg_position_controller_output_get_yaw(msg); + memcpy( position_controller_output, msg->payload, sizeof(mavlink_position_controller_output_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index d6e4da3dd..7d9c425a6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_TARGET PACKING #define MAVLINK_MSG_ID_POSITION_TARGET 63 +#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 +#define MAVLINK_MSG_63_LEN 16 typedef struct __mavlink_position_target_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_position_target_t } mavlink_position_target_t; - - /** * @brief Pack a position_target message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_position_target_t */ 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; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 */ 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; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; 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 + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin * @param yaw yaw orientation in radians, 0 = NORTH */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { 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); + uint16_t checksum; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg.payload[0]; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_TARGET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) +{ + mavlink_header_t hdr; + mavlink_position_target_t payload; + uint16_t checksum; + mavlink_position_target_t *p = &payload; + + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_TARGET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,12 +155,8 @@ 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; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -119,12 +166,8 @@ 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; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -134,12 +177,8 @@ 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; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -149,12 +188,8 @@ 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; + mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -165,8 +200,5 @@ 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) { - 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); + memcpy( position_target, msg->payload, sizeof(mavlink_position_target_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index 4ded0b711..a815b1fa0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -1,6 +1,8 @@ // MESSAGE RAW_IMU PACKING #define MAVLINK_MSG_ID_RAW_IMU 28 +#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_28_LEN 26 typedef struct __mavlink_raw_imu_t { @@ -17,8 +19,6 @@ typedef struct __mavlink_raw_imu_t } mavlink_raw_imu_t; - - /** * @brief Pack a raw_imu message * @param system_id ID of this system @@ -39,21 +39,21 @@ typedef struct __mavlink_raw_imu_t */ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; 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); + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN); } /** @@ -76,21 +76,21 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo */ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; 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); + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN); } /** @@ -122,12 +122,75 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com * @param zmag Z Magnetic field (raw) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { 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); + uint16_t checksum; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RAW_IMU_LEN; + msg.msgid = MAVLINK_MSG_ID_RAW_IMU; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_raw_imu_t payload; + uint16_t checksum; + mavlink_raw_imu_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_IMU_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_IMU; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -140,16 +203,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -159,10 +214,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xacc); } /** @@ -172,10 +225,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->yacc); } /** @@ -185,10 +236,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zacc); } /** @@ -198,10 +247,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xgyro); } /** @@ -211,10 +258,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->ygyro); } /** @@ -224,10 +269,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zgyro); } /** @@ -237,10 +280,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->xmag); } /** @@ -250,10 +291,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->ymag); } /** @@ -263,10 +302,8 @@ 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; + mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; + return (int16_t)(p->zmag); } /** @@ -277,14 +314,5 @@ 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) { - raw_imu->usec = mavlink_msg_raw_imu_get_usec(msg); - raw_imu->xacc = mavlink_msg_raw_imu_get_xacc(msg); - raw_imu->yacc = mavlink_msg_raw_imu_get_yacc(msg); - raw_imu->zacc = mavlink_msg_raw_imu_get_zacc(msg); - raw_imu->xgyro = mavlink_msg_raw_imu_get_xgyro(msg); - raw_imu->ygyro = mavlink_msg_raw_imu_get_ygyro(msg); - raw_imu->zgyro = mavlink_msg_raw_imu_get_zgyro(msg); - raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); - raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); - raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); + memcpy( raw_imu, msg->payload, sizeof(mavlink_raw_imu_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 8346ac628..ccd6c59ac 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -1,6 +1,8 @@ // MESSAGE RAW_PRESSURE PACKING #define MAVLINK_MSG_ID_RAW_PRESSURE 29 +#define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 +#define MAVLINK_MSG_29_LEN 16 typedef struct __mavlink_raw_pressure_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_raw_pressure_t } mavlink_raw_pressure_t; - - /** * @brief Pack a raw_pressure message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_raw_pressure_t */ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t */ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t: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, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ * @param temperature Raw Temperature measurement (raw) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { 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); + uint16_t checksum; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; + msg.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_raw_pressure_t payload; + uint16_t checksum; + mavlink_raw_pressure_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,16 +163,8 @@ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -129,10 +174,8 @@ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_abs); } /** @@ -142,10 +185,8 @@ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_diff1); } /** @@ -155,10 +196,8 @@ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->press_diff2); } /** @@ -168,10 +207,8 @@ 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; + mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; + return (int16_t)(p->temperature); } /** @@ -182,9 +219,5 @@ 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) { - 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); + memcpy( raw_pressure, msg->payload, sizeof(mavlink_raw_pressure_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h index 0443838bd..7f7351d65 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -1,6 +1,8 @@ // MESSAGE RC_CHANNELS_RAW PACKING #define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 17 +#define MAVLINK_MSG_35_LEN 17 typedef struct __mavlink_rc_channels_raw_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_rc_channels_raw_t } mavlink_rc_channels_raw_t; - - /** * @brief Pack a rc_channels_raw message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_rc_channels_raw_t */ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; 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% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t: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, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 */ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; 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% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t: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, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) { 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); + uint16_t checksum; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg.payload[0]; + + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; + msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_rc_channels_raw_t payload; + uint16_t checksum; + mavlink_rc_channels_raw_t *p = &payload; + + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,10 +195,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan1_raw); } /** @@ -147,10 +206,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan2_raw); } /** @@ -160,10 +217,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan3_raw); } /** @@ -173,10 +228,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan4_raw); } /** @@ -186,10 +239,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan5_raw); } /** @@ -199,10 +250,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan6_raw); } /** @@ -212,10 +261,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan7_raw); } /** @@ -225,10 +272,8 @@ 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; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint16_t)(p->chan8_raw); } /** @@ -238,7 +283,8 @@ 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]; + mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; + return (uint8_t)(p->rssi); } /** @@ -249,13 +295,5 @@ 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) { - rc_channels_raw->chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(msg); - rc_channels_raw->chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(msg); - rc_channels_raw->chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(msg); - rc_channels_raw->chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(msg); - rc_channels_raw->chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(msg); - rc_channels_raw->chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(msg); - rc_channels_raw->chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(msg); - rc_channels_raw->chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(msg); - rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); + memcpy( rc_channels_raw, msg->payload, sizeof(mavlink_rc_channels_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h index 526482b4e..16bf4791e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -1,6 +1,8 @@ // MESSAGE RC_CHANNELS_SCALED PACKING #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 17 +#define MAVLINK_MSG_36_LEN 17 typedef struct __mavlink_rc_channels_scaled_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_rc_channels_scaled_t } mavlink_rc_channels_scaled_t; - - /** * @brief Pack a rc_channels_scaled message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_rc_channels_scaled_t */ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; 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% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t: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, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui */ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; 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% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t: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, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { 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); + uint16_t checksum; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg.payload[0]; + + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; + msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_rc_channels_scaled_t payload; + uint16_t checksum; + mavlink_rc_channels_scaled_t *p = &payload; + + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,10 +195,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan1_scaled); } /** @@ -147,10 +206,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan2_scaled); } /** @@ -160,10 +217,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan3_scaled); } /** @@ -173,10 +228,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan4_scaled); } /** @@ -186,10 +239,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan5_scaled); } /** @@ -199,10 +250,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan6_scaled); } /** @@ -212,10 +261,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan7_scaled); } /** @@ -225,10 +272,8 @@ 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; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (int16_t)(p->chan8_scaled); } /** @@ -238,7 +283,8 @@ 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]; + mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; + return (uint8_t)(p->rssi); } /** @@ -249,13 +295,5 @@ 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) { - rc_channels_scaled->chan1_scaled = mavlink_msg_rc_channels_scaled_get_chan1_scaled(msg); - rc_channels_scaled->chan2_scaled = mavlink_msg_rc_channels_scaled_get_chan2_scaled(msg); - rc_channels_scaled->chan3_scaled = mavlink_msg_rc_channels_scaled_get_chan3_scaled(msg); - rc_channels_scaled->chan4_scaled = mavlink_msg_rc_channels_scaled_get_chan4_scaled(msg); - rc_channels_scaled->chan5_scaled = mavlink_msg_rc_channels_scaled_get_chan5_scaled(msg); - rc_channels_scaled->chan6_scaled = mavlink_msg_rc_channels_scaled_get_chan6_scaled(msg); - rc_channels_scaled->chan7_scaled = mavlink_msg_rc_channels_scaled_get_chan7_scaled(msg); - rc_channels_scaled->chan8_scaled = mavlink_msg_rc_channels_scaled_get_chan8_scaled(msg); - rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); + memcpy( rc_channels_scaled, msg->payload, sizeof(mavlink_rc_channels_scaled_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h index f390e4e37..4d48f0954 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -1,6 +1,8 @@ // MESSAGE REQUEST_DATA_STREAM PACKING #define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 +#define MAVLINK_MSG_66_LEN 6 typedef struct __mavlink_request_data_stream_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_request_data_stream_t } mavlink_request_data_stream_t; - - /** * @brief Pack a request_data_stream message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_request_data_stream_t */ 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; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; 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); // The requested interval between two messages of this type - i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t: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, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u */ 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; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; 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); // The requested interval between two messages of this type - i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t: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, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @param start_stop 1 to start sending, 0 to stop sending. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { 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); + uint16_t checksum; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; + msg.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_request_data_stream_t payload; + uint16_t checksum; + mavlink_request_data_stream_t *p = &payload; + + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; + hdr.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ 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]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -120,7 +174,8 @@ 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]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -130,7 +185,8 @@ 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]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->req_stream_id); } /** @@ -140,10 +196,8 @@ 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; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint16_t)(p->req_message_rate); } /** @@ -153,7 +207,8 @@ 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]; + mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; + return (uint8_t)(p->start_stop); } /** @@ -164,9 +219,5 @@ 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) { - 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); + memcpy( request_data_stream, msg->payload, sizeof(mavlink_request_data_stream_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h index e1f9cc6b1..aaee3bf13 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -1,6 +1,8 @@ // MESSAGE SAFETY_ALLOWED_AREA PACKING #define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 +#define MAVLINK_MSG_54_LEN 25 typedef struct __mavlink_safety_allowed_area_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_safety_allowed_area_t } mavlink_safety_allowed_area_t; - - /** * @brief Pack a safety_allowed_area message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_safety_allowed_area_t */ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u */ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float: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, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, * @param p2z z position 2 / Altitude 2 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { mavlink_message_t msg; - mavlink_msg_safety_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg.payload[0]; + + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; + msg.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_safety_allowed_area_t payload; + uint16_t checksum; + mavlink_safety_allowed_area_t *p = &payload; + + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; + hdr.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ 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]; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -132,12 +190,8 @@ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1x); } /** @@ -147,12 +201,8 @@ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1y); } /** @@ -162,12 +212,8 @@ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1z); } /** @@ -177,12 +223,8 @@ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2x); } /** @@ -192,12 +234,8 @@ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2y); } /** @@ -207,12 +245,8 @@ 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; + mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2z); } /** @@ -223,11 +257,5 @@ 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) { - safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); - safety_allowed_area->p1x = mavlink_msg_safety_allowed_area_get_p1x(msg); - safety_allowed_area->p1y = mavlink_msg_safety_allowed_area_get_p1y(msg); - safety_allowed_area->p1z = mavlink_msg_safety_allowed_area_get_p1z(msg); - safety_allowed_area->p2x = mavlink_msg_safety_allowed_area_get_p2x(msg); - safety_allowed_area->p2y = mavlink_msg_safety_allowed_area_get_p2y(msg); - safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); + memcpy( safety_allowed_area, msg->payload, sizeof(mavlink_safety_allowed_area_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h index da571e8e7..4427dbedd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h @@ -1,6 +1,8 @@ // MESSAGE SAFETY_SET_ALLOWED_AREA PACKING #define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 +#define MAVLINK_MSG_53_LEN 27 typedef struct __mavlink_safety_set_allowed_area_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_safety_set_allowed_area_t } mavlink_safety_set_allowed_area_t; - - /** * @brief Pack a safety_set_allowed_area message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_safety_set_allowed_area_t */ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i */ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID - i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID - i += put_uint8_t_by_index(frame, i, msg->payload); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - i += put_float_by_index(p1x, i, msg->payload); // x position 1 / Latitude 1 - i += put_float_by_index(p1y, i, msg->payload); // y position 1 / Longitude 1 - i += put_float_by_index(p1z, i, msg->payload); // z position 1 / Altitude 1 - i += put_float_by_index(p2x, i, msg->payload); // x position 2 / Latitude 2 - i += put_float_by_index(p2y, i, msg->payload); // y position 2 / Longitude 2 - i += put_float_by_index(p2z, i, msg->payload); // z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float: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, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system * @param p2z z position 2 / Altitude 2 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { mavlink_message_t msg; - mavlink_msg_safety_set_allowed_area_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; + msg.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_safety_set_allowed_area_t payload; + uint16_t checksum; + mavlink_safety_set_allowed_area_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t: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. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; + hdr.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +195,8 @@ 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]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -144,7 +206,8 @@ 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]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -154,7 +217,8 @@ 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]; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -164,12 +228,8 @@ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1x); } /** @@ -179,12 +239,8 @@ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1y); } /** @@ -194,12 +250,8 @@ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p1z); } /** @@ -209,12 +261,8 @@ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2x); } /** @@ -224,12 +272,8 @@ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2y); } /** @@ -239,12 +283,8 @@ 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; + mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; + return (float)(p->p2z); } /** @@ -255,13 +295,5 @@ 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) { - safety_set_allowed_area->target_system = mavlink_msg_safety_set_allowed_area_get_target_system(msg); - safety_set_allowed_area->target_component = mavlink_msg_safety_set_allowed_area_get_target_component(msg); - safety_set_allowed_area->frame = mavlink_msg_safety_set_allowed_area_get_frame(msg); - safety_set_allowed_area->p1x = mavlink_msg_safety_set_allowed_area_get_p1x(msg); - safety_set_allowed_area->p1y = mavlink_msg_safety_set_allowed_area_get_p1y(msg); - safety_set_allowed_area->p1z = mavlink_msg_safety_set_allowed_area_get_p1z(msg); - safety_set_allowed_area->p2x = mavlink_msg_safety_set_allowed_area_get_p2x(msg); - safety_set_allowed_area->p2y = mavlink_msg_safety_set_allowed_area_get_p2y(msg); - safety_set_allowed_area->p2z = mavlink_msg_safety_set_allowed_area_get_p2z(msg); + memcpy( safety_set_allowed_area, msg->payload, sizeof(mavlink_safety_set_allowed_area_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 77637286f..44fe8bdbd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -1,6 +1,8 @@ // MESSAGE SCALED_IMU PACKING #define MAVLINK_MSG_ID_SCALED_IMU 26 +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 26 +#define MAVLINK_MSG_26_LEN 26 typedef struct __mavlink_scaled_imu_t { @@ -17,8 +19,6 @@ typedef struct __mavlink_scaled_imu_t } mavlink_scaled_imu_t; - - /** * @brief Pack a scaled_imu message * @param system_id ID of this system @@ -39,21 +39,21 @@ typedef struct __mavlink_scaled_imu_t */ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; 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); + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN); } /** @@ -76,21 +76,21 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co */ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; 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); + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN); } /** @@ -122,12 +122,75 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t * @param zmag Z Magnetic field (milli tesla) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { 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); + uint16_t checksum; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; + msg.msgid = MAVLINK_MSG_ID_SCALED_IMU; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_scaled_imu_t payload; + uint16_t checksum; + mavlink_scaled_imu_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; + hdr.msgid = MAVLINK_MSG_ID_SCALED_IMU; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -140,16 +203,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -159,10 +214,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xacc); } /** @@ -172,10 +225,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->yacc); } /** @@ -185,10 +236,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zacc); } /** @@ -198,10 +247,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xgyro); } /** @@ -211,10 +258,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->ygyro); } /** @@ -224,10 +269,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zgyro); } /** @@ -237,10 +280,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->xmag); } /** @@ -250,10 +291,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->ymag); } /** @@ -263,10 +302,8 @@ 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; + mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; + return (int16_t)(p->zmag); } /** @@ -277,14 +314,5 @@ 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) { - scaled_imu->usec = mavlink_msg_scaled_imu_get_usec(msg); - scaled_imu->xacc = mavlink_msg_scaled_imu_get_xacc(msg); - scaled_imu->yacc = mavlink_msg_scaled_imu_get_yacc(msg); - scaled_imu->zacc = mavlink_msg_scaled_imu_get_zacc(msg); - scaled_imu->xgyro = mavlink_msg_scaled_imu_get_xgyro(msg); - scaled_imu->ygyro = mavlink_msg_scaled_imu_get_ygyro(msg); - scaled_imu->zgyro = mavlink_msg_scaled_imu_get_zgyro(msg); - scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); - scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); - scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); + memcpy( scaled_imu, msg->payload, sizeof(mavlink_scaled_imu_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 08a9bedd3..bf653ad5a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -1,6 +1,8 @@ // MESSAGE SCALED_PRESSURE PACKING #define MAVLINK_MSG_ID_SCALED_PRESSURE 38 +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 18 +#define MAVLINK_MSG_38_LEN 18 typedef struct __mavlink_scaled_pressure_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_scaled_pressure_t } mavlink_scaled_pressure_t; - - /** * @brief Pack a scaled_pressure message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_scaled_pressure_t */ 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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t: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, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 */ 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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; 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) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t: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, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin * @param temperature Temperature measurement (0.01 degrees celsius) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { 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); + uint16_t checksum; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; + msg.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_scaled_pressure_t payload; + uint16_t checksum; + mavlink_scaled_pressure_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,16 +155,8 @@ 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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -123,12 +166,8 @@ 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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (float)(p->press_abs); } /** @@ -138,12 +177,8 @@ 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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (float)(p->press_diff); } /** @@ -153,10 +188,8 @@ 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; + mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; + return (int16_t)(p->temperature); } /** @@ -167,8 +200,5 @@ 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) { - 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); + memcpy( scaled_pressure, msg->payload, sizeof(mavlink_scaled_pressure_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h index 751afcb80..f12eb29a0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -1,6 +1,8 @@ // MESSAGE SERVO_OUTPUT_RAW PACKING #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 16 +#define MAVLINK_MSG_37_LEN 16 typedef struct __mavlink_servo_output_raw_t { @@ -15,8 +17,6 @@ typedef struct __mavlink_servo_output_raw_t } mavlink_servo_output_raw_t; - - /** * @brief Pack a servo_output_raw message * @param system_id ID of this system @@ -35,19 +35,19 @@ typedef struct __mavlink_servo_output_raw_t */ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t: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, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } /** @@ -68,19 +68,19 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint */ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - i += put_uint16_t_by_index(servo1_raw, i, msg->payload); // Servo output 1 value, in microseconds - i += put_uint16_t_by_index(servo2_raw, i, msg->payload); // Servo output 2 value, in microseconds - i += put_uint16_t_by_index(servo3_raw, i, msg->payload); // Servo output 3 value, in microseconds - i += put_uint16_t_by_index(servo4_raw, i, msg->payload); // Servo output 4 value, in microseconds - i += put_uint16_t_by_index(servo5_raw, i, msg->payload); // Servo output 5 value, in microseconds - i += put_uint16_t_by_index(servo6_raw, i, msg->payload); // Servo output 6 value, in microseconds - i += put_uint16_t_by_index(servo7_raw, i, msg->payload); // Servo output 7 value, in microseconds - i += put_uint16_t_by_index(servo8_raw, i, msg->payload); // Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t: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, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } /** @@ -110,12 +110,71 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui * @param servo8_raw Servo output 8 value, in microseconds */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { mavlink_message_t msg; - mavlink_msg_servo_output_raw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg.payload[0]; + + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; + msg.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_servo_output_raw_t payload; + uint16_t checksum; + mavlink_servo_output_raw_t *p = &payload; + + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -128,10 +187,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo1_raw); } /** @@ -141,10 +198,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo2_raw); } /** @@ -154,10 +209,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo3_raw); } /** @@ -167,10 +220,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo4_raw); } /** @@ -180,10 +231,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo5_raw); } /** @@ -193,10 +242,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo6_raw); } /** @@ -206,10 +253,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo7_raw); } /** @@ -219,10 +264,8 @@ 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; + mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; + return (uint16_t)(p->servo8_raw); } /** @@ -233,12 +276,5 @@ 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) { - servo_output_raw->servo1_raw = mavlink_msg_servo_output_raw_get_servo1_raw(msg); - servo_output_raw->servo2_raw = mavlink_msg_servo_output_raw_get_servo2_raw(msg); - servo_output_raw->servo3_raw = mavlink_msg_servo_output_raw_get_servo3_raw(msg); - servo_output_raw->servo4_raw = mavlink_msg_servo_output_raw_get_servo4_raw(msg); - servo_output_raw->servo5_raw = mavlink_msg_servo_output_raw_get_servo5_raw(msg); - servo_output_raw->servo6_raw = mavlink_msg_servo_output_raw_get_servo6_raw(msg); - servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); - servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); + memcpy( servo_output_raw, msg->payload, sizeof(mavlink_servo_output_raw_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 775db90e3..3e32eee15 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -1,6 +1,8 @@ // MESSAGE SET_ALTITUDE PACKING #define MAVLINK_MSG_ID_SET_ALTITUDE 65 +#define MAVLINK_MSG_ID_SET_ALTITUDE_LEN 5 +#define MAVLINK_MSG_65_LEN 5 typedef struct __mavlink_set_altitude_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_altitude_t } mavlink_set_altitude_t; - - /** * @brief Pack a set_altitude message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_altitude_t */ 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; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t */ 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; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t: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, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ * @param mode The new altitude in meters */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) { 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); + uint16_t checksum; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) +{ + mavlink_header_t hdr; + mavlink_set_altitude_t payload; + uint16_t checksum; + mavlink_set_altitude_t *p = &payload; + + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,12 +150,8 @@ 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; + mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; + return (uint32_t)(p->mode); } /** @@ -118,6 +162,5 @@ 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) { - set_altitude->target = mavlink_msg_set_altitude_get_target(msg); - set_altitude->mode = mavlink_msg_set_altitude_get_mode(msg); + memcpy( set_altitude, msg->payload, sizeof(mavlink_set_altitude_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index efeea6509..a74108ee8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -1,6 +1,8 @@ // MESSAGE SET_MODE PACKING #define MAVLINK_MSG_ID_SET_MODE 11 +#define MAVLINK_MSG_ID_SET_MODE_LEN 2 +#define MAVLINK_MSG_11_LEN 2 typedef struct __mavlink_set_mode_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_mode_t } mavlink_set_mode_t; - - /** * @brief Pack a set_mode message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_mode_t */ 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; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp */ 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; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t: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, MAVLINK_MSG_ID_SET_MODE_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co * @param mode The new mode */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { 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); + uint16_t checksum; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_MODE_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_MODE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) +{ + mavlink_header_t hdr; + mavlink_set_mode_t payload; + uint16_t checksum; + mavlink_set_mode_t *p = &payload; + + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_MODE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_MODE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,7 +150,8 @@ 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]; + mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -113,6 +162,5 @@ 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) { - set_mode->target = mavlink_msg_set_mode_get_target(msg); - set_mode->mode = mavlink_msg_set_mode_get_mode(msg); + memcpy( set_mode, msg->payload, sizeof(mavlink_set_mode_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h index fe514b2ea..649fbee57 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -1,6 +1,8 @@ // MESSAGE SET_NAV_MODE PACKING #define MAVLINK_MSG_ID_SET_NAV_MODE 12 +#define MAVLINK_MSG_ID_SET_NAV_MODE_LEN 2 +#define MAVLINK_MSG_12_LEN 2 typedef struct __mavlink_set_nav_mode_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_set_nav_mode_t } mavlink_set_nav_mode_t; - - /** * @brief Pack a set_nav_mode message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_set_nav_mode_t */ 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; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t */ 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; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; 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 + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t: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, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ * @param nav_mode The new navigation mode */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) { 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); + uint16_t checksum; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) +{ + mavlink_header_t hdr; + mavlink_set_nav_mode_t payload; + uint16_t checksum; + mavlink_set_nav_mode_t *p = &payload; + + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,7 +150,8 @@ 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]; + mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -113,6 +162,5 @@ 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) { - 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); + memcpy( set_nav_mode, msg->payload, sizeof(mavlink_set_nav_mode_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index c4c6b5abc..6ca16a534 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -1,6 +1,8 @@ // MESSAGE STATE_CORRECTION PACKING #define MAVLINK_MSG_ID_STATE_CORRECTION 64 +#define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 +#define MAVLINK_MSG_64_LEN 36 typedef struct __mavlink_state_correction_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_state_correction_t } mavlink_state_correction_t; - - /** * @brief Pack a state_correction message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_state_correction_t */ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; 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 + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint */ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; 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 + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float: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, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui * @param vzErr z velocity */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { 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); + uint16_t checksum; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg.payload[0]; + + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; + msg.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_state_correction_t payload; + uint16_t checksum; + mavlink_state_correction_t *p = &payload; + + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,12 +195,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->xErr); } /** @@ -149,12 +206,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->yErr); } /** @@ -164,12 +217,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->zErr); } /** @@ -179,12 +228,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->rollErr); } /** @@ -194,12 +239,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->pitchErr); } /** @@ -209,12 +250,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->yawErr); } /** @@ -224,12 +261,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vxErr); } /** @@ -239,12 +272,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vyErr); } /** @@ -254,12 +283,8 @@ 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; + mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; + return (float)(p->vzErr); } /** @@ -270,13 +295,5 @@ 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) { - state_correction->xErr = mavlink_msg_state_correction_get_xErr(msg); - state_correction->yErr = mavlink_msg_state_correction_get_yErr(msg); - state_correction->zErr = mavlink_msg_state_correction_get_zErr(msg); - state_correction->rollErr = mavlink_msg_state_correction_get_rollErr(msg); - state_correction->pitchErr = mavlink_msg_state_correction_get_pitchErr(msg); - state_correction->yawErr = mavlink_msg_state_correction_get_yawErr(msg); - state_correction->vxErr = mavlink_msg_state_correction_get_vxErr(msg); - state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); - state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); + memcpy( state_correction, msg->payload, sizeof(mavlink_state_correction_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index 681b659c4..485bbf1cc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -1,17 +1,17 @@ // MESSAGE STATUSTEXT PACKING #define MAVLINK_MSG_ID_STATUSTEXT 254 +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_254_LEN 51 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 + char text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; - #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 - /** * @brief Pack a statustext message * @param system_id ID of this system @@ -22,15 +22,15 @@ 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 char* text) { - uint16_t i = 0; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; 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 + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]: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, MAVLINK_MSG_ID_STATUSTEXT_LEN); } /** @@ -43,15 +43,15 @@ 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 char* text) { - uint16_t i = 0; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; 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 + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]: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, MAVLINK_MSG_ID_STATUSTEXT_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param text Status text message, without null termination character */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text) +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* 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); + uint16_t checksum; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg.payload[0]; + + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; + msg.msgid = MAVLINK_MSG_ID_STATUSTEXT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) +{ + mavlink_header_t hdr; + mavlink_statustext_t payload; + uint16_t checksum; + mavlink_statustext_t *p = &payload; + + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; + hdr.msgid = MAVLINK_MSG_ID_STATUSTEXT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -93,7 +140,8 @@ 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]; + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; + return (uint8_t)(p->severity); } /** @@ -101,11 +149,12 @@ 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, char* text) { + mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t), sizeof(int8_t)*50); - return sizeof(int8_t)*50; + memcpy(text, p->text, sizeof(p->text)); + return sizeof(p->text); } /** @@ -116,6 +165,5 @@ 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) { - statustext->severity = mavlink_msg_statustext_get_severity(msg); - mavlink_msg_statustext_get_text(msg, statustext->text); + memcpy( statustext, msg->payload, sizeof(mavlink_statustext_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index ef83d8448..a340cfaa8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -1,6 +1,8 @@ // MESSAGE SYS_STATUS PACKING #define MAVLINK_MSG_ID_SYS_STATUS 34 +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 11 +#define MAVLINK_MSG_34_LEN 11 typedef struct __mavlink_sys_status_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_sys_status_t } mavlink_sys_status_t; - - /** * @brief Pack a sys_status message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_sys_status_t */ 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; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; 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) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t: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, MAVLINK_MSG_ID_SYS_STATUS_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co */ 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; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; 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) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t: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, MAVLINK_MSG_ID_SYS_STATUS_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { 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); + uint16_t checksum; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg.payload[0]; + + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_SYS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_sys_status_t payload; + uint16_t checksum; + mavlink_sys_status_t *p = &payload; + + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ 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]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -132,7 +190,8 @@ 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]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -142,7 +201,8 @@ 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]; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->status); } /** @@ -152,10 +212,8 @@ 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; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->load); } /** @@ -165,10 +223,8 @@ 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; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->vbat); } /** @@ -178,10 +234,8 @@ 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; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->battery_remaining); } /** @@ -191,10 +245,8 @@ 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; + mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; + return (uint16_t)(p->packet_drop); } /** @@ -205,11 +257,5 @@ 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) { - sys_status->mode = mavlink_msg_sys_status_get_mode(msg); - sys_status->nav_mode = mavlink_msg_sys_status_get_nav_mode(msg); - sys_status->status = mavlink_msg_sys_status_get_status(msg); - sys_status->load = mavlink_msg_sys_status_get_load(msg); - sys_status->vbat = mavlink_msg_sys_status_get_vbat(msg); - sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); - sys_status->packet_drop = mavlink_msg_sys_status_get_packet_drop(msg); + memcpy( sys_status, msg->payload, sizeof(mavlink_sys_status_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index d293f025b..2213495fc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -1,6 +1,8 @@ // MESSAGE SYSTEM_TIME PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME 2 +#define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 8 +#define MAVLINK_MSG_2_LEN 8 typedef struct __mavlink_system_time_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_system_time_t } mavlink_system_time_t; - - /** * @brief Pack a system_time message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_system_time_t */ 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; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; 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. + p->time_usec = time_usec; // uint64_t: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, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c */ 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; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; 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. + p->time_usec = time_usec; // uint64_t: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, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { 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); + uint16_t checksum; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg.payload[0]; + + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; + msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) +{ + mavlink_header_t hdr; + mavlink_system_time_t payload; + uint16_t checksum; + mavlink_system_time_t *p = &payload; + + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,16 +131,8 @@ 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; + mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; + return (uint64_t)(p->time_usec); } /** @@ -106,5 +143,5 @@ 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) { - system_time->time_usec = mavlink_msg_system_time_get_time_usec(msg); + memcpy( system_time, msg->payload, sizeof(mavlink_system_time_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h index 75c25f1ce..04d124f2b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -1,6 +1,8 @@ // MESSAGE SYSTEM_TIME_UTC PACKING #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 +#define MAVLINK_MSG_4_LEN 8 typedef struct __mavlink_system_time_utc_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_system_time_utc_t } mavlink_system_time_utc_t; - - /** * @brief Pack a system_time_utc message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_system_time_utc_t */ 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; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 */ 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; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - i += put_uint32_t_by_index(utc_date, i, msg->payload); // GPS UTC date ddmmyy - i += put_uint32_t_by_index(utc_time, i, msg->payload); // GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t: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, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin * @param utc_time GPS UTC time hhmmss */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { mavlink_message_t msg; - mavlink_msg_system_time_utc_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, utc_date, utc_time); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg.payload[0]; + + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; + msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) +{ + mavlink_header_t hdr; + mavlink_system_time_utc_t payload; + uint16_t checksum; + mavlink_system_time_utc_t *p = &payload; + + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; + hdr.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,12 +139,8 @@ 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; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; + return (uint32_t)(p->utc_date); } /** @@ -107,12 +150,8 @@ 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; + mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; + return (uint32_t)(p->utc_time); } /** @@ -123,6 +162,5 @@ 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) { - 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); + memcpy( system_time_utc, msg->payload, sizeof(mavlink_system_time_utc_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index 9f55bdfa7..ee92a1e34 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -1,6 +1,8 @@ // MESSAGE VFR_HUD PACKING #define MAVLINK_MSG_ID_VFR_HUD 74 +#define MAVLINK_MSG_ID_VFR_HUD_LEN 20 +#define MAVLINK_MSG_74_LEN 20 typedef struct __mavlink_vfr_hud_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_vfr_hud_t } mavlink_vfr_hud_t; - - /** * @brief Pack a vfr_hud message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_vfr_hud_t */ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, 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 + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float: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, MAVLINK_MSG_ID_VFR_HUD_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo */ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - i += put_float_by_index(airspeed, i, msg->payload); // Current airspeed in m/s - i += put_float_by_index(groundspeed, i, msg->payload); // Current ground speed in m/s - i += put_int16_t_by_index(heading, i, msg->payload); // Current heading in degrees, 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 + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float: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, MAVLINK_MSG_ID_VFR_HUD_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com * @param climb Current climb rate in meters/second */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { 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); + uint16_t checksum; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg.payload[0]; + + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VFR_HUD_LEN; + msg.msgid = MAVLINK_MSG_ID_VFR_HUD; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_vfr_hud_t payload; + uint16_t checksum; + mavlink_vfr_hud_t *p = &payload; + + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VFR_HUD_LEN; + hdr.msgid = MAVLINK_MSG_ID_VFR_HUD; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->airspeed); } /** @@ -131,12 +182,8 @@ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->groundspeed); } /** @@ -146,10 +193,8 @@ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (int16_t)(p->heading); } /** @@ -159,10 +204,8 @@ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (uint16_t)(p->throttle); } /** @@ -172,12 +215,8 @@ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -187,12 +226,8 @@ 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; + mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; + return (float)(p->climb); } /** @@ -203,10 +238,5 @@ 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) { - 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); + memcpy( vfr_hud, msg->payload, sizeof(mavlink_vfr_hud_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index ab1c5f836..07f7a08bd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT PACKING #define MAVLINK_MSG_ID_WAYPOINT 39 +#define MAVLINK_MSG_ID_WAYPOINT_LEN 36 +#define MAVLINK_MSG_39_LEN 36 typedef struct __mavlink_waypoint_t { @@ -21,8 +23,6 @@ typedef struct __mavlink_waypoint_t } mavlink_waypoint_t; - - /** * @brief Pack a waypoint message * @param system_id ID of this system @@ -47,25 +47,25 @@ typedef struct __mavlink_waypoint_t */ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; 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); + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_LEN); } /** @@ -92,25 +92,25 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp */ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; 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); + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_LEN); } /** @@ -146,12 +146,83 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co * @param z PARAM7 / z position: global: altitude */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { 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); + uint16_t checksum; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_waypoint_t payload; + uint16_t checksum; + mavlink_waypoint_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float: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. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -164,7 +235,8 @@ 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]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -174,7 +246,8 @@ 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]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -184,10 +257,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -197,7 +268,8 @@ 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]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->frame); } /** @@ -207,7 +279,8 @@ 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]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->command); } /** @@ -217,7 +290,8 @@ 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]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->current); } /** @@ -227,7 +301,8 @@ 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]; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (uint8_t)(p->autocontinue); } /** @@ -237,12 +312,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param1); } /** @@ -252,12 +323,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param2); } /** @@ -267,12 +334,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param3); } /** @@ -282,12 +345,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->param4); } /** @@ -297,12 +356,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -312,12 +367,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -327,12 +378,8 @@ 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; + mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -343,18 +390,5 @@ 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) { - waypoint->target_system = mavlink_msg_waypoint_get_target_system(msg); - waypoint->target_component = mavlink_msg_waypoint_get_target_component(msg); - waypoint->seq = mavlink_msg_waypoint_get_seq(msg); - waypoint->frame = mavlink_msg_waypoint_get_frame(msg); - waypoint->command = mavlink_msg_waypoint_get_command(msg); - waypoint->current = mavlink_msg_waypoint_get_current(msg); - waypoint->autocontinue = mavlink_msg_waypoint_get_autocontinue(msg); - waypoint->param1 = mavlink_msg_waypoint_get_param1(msg); - waypoint->param2 = mavlink_msg_waypoint_get_param2(msg); - waypoint->param3 = mavlink_msg_waypoint_get_param3(msg); - waypoint->param4 = mavlink_msg_waypoint_get_param4(msg); - waypoint->x = mavlink_msg_waypoint_get_x(msg); - waypoint->y = mavlink_msg_waypoint_get_y(msg); - waypoint->z = mavlink_msg_waypoint_get_z(msg); + memcpy( waypoint, msg->payload, sizeof(mavlink_waypoint_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index d994eaf49..8bdd5ae15 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_ACK PACKING #define MAVLINK_MSG_ID_WAYPOINT_ACK 47 +#define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 +#define MAVLINK_MSG_47_LEN 3 typedef struct __mavlink_waypoint_ack_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_ack_t } mavlink_waypoint_ack_t; - - /** * @brief Pack a waypoint_ack message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_ack_t */ 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; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t */ 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; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t: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, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ * @param type 0: OK, 1: Error */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { 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); + uint16_t checksum; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +{ + mavlink_header_t hdr; + mavlink_waypoint_ack_t payload; + uint16_t checksum; + mavlink_waypoint_ack_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ 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]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ 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]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,7 +169,8 @@ 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]; + mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -129,7 +181,5 @@ 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) { - 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); + memcpy( waypoint_ack, msg->payload, sizeof(mavlink_waypoint_ack_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h index 5020fbada..efccf3600 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_CLEAR_ALL PACKING #define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 +#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_45_LEN 2 typedef struct __mavlink_waypoint_clear_all_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_waypoint_clear_all_t } mavlink_waypoint_clear_all_t; - - /** * @brief Pack a waypoint_clear_all message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_waypoint_clear_all_t */ 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; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui */ 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; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t: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, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, * @param target_component Component ID */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { 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); + uint16_t checksum; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ + mavlink_header_t hdr; + mavlink_waypoint_clear_all_t payload; + uint16_t checksum; + mavlink_waypoint_clear_all_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +150,8 @@ 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]; + mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +162,5 @@ 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) { - 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); + memcpy( waypoint_clear_all, msg->payload, sizeof(mavlink_waypoint_clear_all_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 9ceacb080..a44619721 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_COUNT PACKING #define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 +#define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 +#define MAVLINK_MSG_44_LEN 4 typedef struct __mavlink_waypoint_count_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_count_t } mavlink_waypoint_count_t; - - /** * @brief Pack a waypoint_count message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_count_t */ 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; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t: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, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ */ 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; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t: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, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint * @param count Number of Waypoints in the Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { 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); + uint16_t checksum; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +{ + mavlink_header_t hdr; + mavlink_waypoint_count_t payload; + uint16_t checksum; + mavlink_waypoint_count_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ 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]; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ 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]; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +169,8 @@ 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; + mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; + return (uint16_t)(p->count); } /** @@ -132,7 +181,5 @@ 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) { - 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); + memcpy( waypoint_count, msg->payload, sizeof(mavlink_waypoint_count_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 17f66fc29..59b2f6b3a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_CURRENT PACKING #define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 +#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 +#define MAVLINK_MSG_42_LEN 2 typedef struct __mavlink_waypoint_current_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_waypoint_current_t } mavlink_waypoint_current_t; - - /** * @brief Pack a waypoint_current message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_waypoint_current_t */ 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; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint */ 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; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui * @param seq Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - 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); + uint16_t checksum; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg.payload[0]; + + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) +{ + mavlink_header_t hdr; + mavlink_waypoint_current_t payload; + uint16_t checksum; + mavlink_waypoint_current_t *p = &payload; + + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,10 +131,8 @@ 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; + mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -100,5 +143,5 @@ 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) { - waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); + memcpy( waypoint_current, msg->payload, sizeof(mavlink_waypoint_current_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index ffcea52fc..17404e658 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REACHED PACKING #define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 +#define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 +#define MAVLINK_MSG_46_LEN 2 typedef struct __mavlink_waypoint_reached_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_waypoint_reached_t } mavlink_waypoint_reached_t; - - /** * @brief Pack a waypoint_reached message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_waypoint_reached_t */ 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; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint */ 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; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - i += put_uint16_t_by_index(seq, i, msg->payload); // Sequence + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui * @param seq Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - 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); + uint16_t checksum; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg.payload[0]; + + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) +{ + mavlink_header_t hdr; + mavlink_waypoint_reached_t payload; + uint16_t checksum; + mavlink_waypoint_reached_t *p = &payload; + + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,10 +131,8 @@ 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; + mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -100,5 +143,5 @@ 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) { - waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); + memcpy( waypoint_reached, msg->payload, sizeof(mavlink_waypoint_reached_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index 234b55c11..c33edce8e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REQUEST PACKING #define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 +#define MAVLINK_MSG_40_LEN 4 typedef struct __mavlink_waypoint_request_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_request_t } mavlink_waypoint_request_t; - - /** * @brief Pack a waypoint_request message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_request_t */ 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; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint */ 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; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui * @param seq Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { 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); + uint16_t checksum; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +{ + mavlink_header_t hdr; + mavlink_waypoint_request_t payload; + uint16_t checksum; + mavlink_waypoint_request_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ 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]; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ 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]; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +169,8 @@ 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; + mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -132,7 +181,5 @@ 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) { - 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); + memcpy( waypoint_request, msg->payload, sizeof(mavlink_waypoint_request_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h index d4f7cf03d..209b6140e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_REQUEST_LIST PACKING #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_43_LEN 2 typedef struct __mavlink_waypoint_request_list_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_waypoint_request_list_t } mavlink_waypoint_request_list_t; - - /** * @brief Pack a waypoint_request_list message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_waypoint_request_list_t */ 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; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, */ 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; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t: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, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i * @param target_component Component ID */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { 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); + uint16_t checksum; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ + mavlink_header_t hdr; + mavlink_waypoint_request_list_t payload; + uint16_t checksum; + mavlink_waypoint_request_list_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ 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]; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -102,7 +150,8 @@ 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]; + mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -113,6 +162,5 @@ 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) { - 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); + memcpy( waypoint_request_list, msg->payload, sizeof(mavlink_waypoint_request_list_t)); } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h index 6570fe4f8..c35fe2107 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -1,6 +1,8 @@ // MESSAGE WAYPOINT_SET_CURRENT PACKING #define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 +#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 +#define MAVLINK_MSG_41_LEN 4 typedef struct __mavlink_waypoint_set_current_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_waypoint_set_current_t } mavlink_waypoint_set_current_t; - - /** * @brief Pack a waypoint_set_current message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_waypoint_set_current_t */ 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; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, */ 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; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id * @param seq Sequence */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { 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); + uint16_t checksum; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; + msg.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +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_header_t hdr; + mavlink_waypoint_set_current_t payload; + uint16_t checksum; + mavlink_waypoint_set_current_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ 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]; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -108,7 +158,8 @@ 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]; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -118,10 +169,8 @@ 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; + mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; + return (uint16_t)(p->seq); } /** @@ -132,7 +181,5 @@ 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) { - 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); + memcpy( waypoint_set_current, msg->payload, sizeof(mavlink_waypoint_set_current_t)); } diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index 67eed17c8..199fc56f4 100644 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -3,172 +3,7 @@ #include "inttypes.h" -enum MAV_CLASS -{ - MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything - 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 -}; - -enum MAV_ACTION -{ - MAV_ACTION_HOLD = 0, - MAV_ACTION_MOTORS_START = 1, - MAV_ACTION_LAUNCH = 2, - MAV_ACTION_RETURN = 3, - MAV_ACTION_EMCY_LAND = 4, - MAV_ACTION_EMCY_KILL = 5, - MAV_ACTION_CONFIRM_KILL = 6, - MAV_ACTION_CONTINUE = 7, - MAV_ACTION_MOTORS_STOP = 8, - MAV_ACTION_HALT = 9, - MAV_ACTION_SHUTDOWN = 10, - MAV_ACTION_REBOOT = 11, - MAV_ACTION_SET_MANUAL = 12, - MAV_ACTION_SET_AUTO = 13, - MAV_ACTION_STORAGE_READ = 14, - MAV_ACTION_STORAGE_WRITE = 15, - MAV_ACTION_CALIBRATE_RC = 16, - MAV_ACTION_CALIBRATE_GYRO = 17, - MAV_ACTION_CALIBRATE_MAG = 18, - MAV_ACTION_CALIBRATE_ACC = 19, - MAV_ACTION_CALIBRATE_PRESSURE = 20, - MAV_ACTION_REC_START = 21, - MAV_ACTION_REC_PAUSE = 22, - MAV_ACTION_REC_STOP = 23, - MAV_ACTION_TAKEOFF = 24, - MAV_ACTION_NAVIGATE = 25, - MAV_ACTION_LAND = 26, - MAV_ACTION_LOITER = 27, - MAV_ACTION_SET_ORIGIN = 28, - MAV_ACTION_RELAY_ON = 29, - MAV_ACTION_RELAY_OFF = 30, - MAV_ACTION_GET_IMAGE = 31, - MAV_ACTION_VIDEO_START = 32, - MAV_ACTION_VIDEO_STOP = 33, - MAV_ACTION_RESET_MAP = 34, - MAV_ACTION_RESET_PLAN = 35, - MAV_ACTION_DELAY_BEFORE_COMMAND = 36, - MAV_ACTION_ASCEND_AT_RATE = 37, - MAV_ACTION_CHANGE_MODE = 38, - MAV_ACTION_LOITER_MAX_TURNS = 39, - MAV_ACTION_LOITER_MAX_TIME = 40, - MAV_ACTION_START_HILSIM = 41, - MAV_ACTION_STOP_HILSIM = 42, - MAV_ACTION_NB ///< Number of MAV actions -}; - -enum MAV_MODE -{ - MAV_MODE_UNINIT = 0, ///< System is in undefined state - MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe - MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control - MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint - MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation - MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use - MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use - MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use - MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive - MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back -}; - -enum MAV_STATE -{ - MAV_STATE_UNINIT = 0, - MAV_STATE_BOOT, - MAV_STATE_CALIBRATING, - MAV_STATE_STANDBY, - MAV_STATE_ACTIVE, - MAV_STATE_CRITICAL, - MAV_STATE_EMERGENCY, - MAV_STATE_HILSIM, - MAV_STATE_POWEROFF -}; - -enum MAV_NAV -{ - MAV_NAV_GROUNDED = 0, - MAV_NAV_LIFTOFF, - MAV_NAV_HOLD, - MAV_NAV_WAYPOINT, - MAV_NAV_VECTOR, - MAV_NAV_RETURNING, - MAV_NAV_LANDING, - MAV_NAV_LOST, - MAV_NAV_LOITER, - MAV_NAV_FREE_DRIFT -}; - -enum MAV_TYPE -{ - MAV_GENERIC = 0, - MAV_FIXED_WING = 1, - MAV_QUADROTOR = 2, - MAV_COAXIAL = 3, - MAV_HELICOPTER = 4, - MAV_GROUND = 5, - OCU = 6, - MAV_AIRSHIP = 7, - MAV_FREE_BALLOON = 8, - MAV_ROCKET = 9, - UGV_GROUND_ROVER = 10, - UGV_SURFACE_SHIP = 11 -}; - -enum MAV_AUTOPILOT_TYPE -{ - MAV_AUTOPILOT_GENERIC = 0, - MAV_AUTOPILOT_PIXHAWK = 1, - MAV_AUTOPILOT_SLUGS = 2, - MAV_AUTOPILOT_ARDUPILOTMEGA = 3, - MAV_AUTOPILOT_NONE = 4 -}; - -enum MAV_COMPONENT -{ - MAV_COMP_ID_GPS, - MAV_COMP_ID_WAYPOINTPLANNER, - MAV_COMP_ID_BLOBTRACKER, - MAV_COMP_ID_PATHPLANNER, - MAV_COMP_ID_AIRSLAM, - MAV_COMP_ID_MAPPER, - MAV_COMP_ID_CAMERA, - MAV_COMP_ID_IMU = 200, - 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 -}; - -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 -}; - -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 - -}; - -#define MAVLINK_STX 0x55 ///< Packet start sign +#define MAVLINK_STX 0xD5 ///< Packet start sign #define MAVLINK_STX_LEN 1 ///< Length of start sign #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length @@ -191,36 +26,37 @@ typedef struct __mavlink_system { } mavlink_system_t; typedef struct __mavlink_message { + uint8_t ck_a; ///< Checksum high byte + uint8_t ck_b; ///< Checksum low byte + uint8_t STX; ///< start of packet 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 uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header { uint8_t ck_a; ///< Checksum high byte uint8_t ck_b; ///< Checksum low byte -} mavlink_message_t; + uint8_t STX; ///< start of packet 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 +} mavlink_header_t; typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, - MAVLINK_COMM_3 + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 } mavlink_channel_t; -/* - * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number - * of buffers they will use. If more are used, then the result will be - * a stack overrun - */ -#ifndef MAVLINK_COMM_NUM_BUFFERS -#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) -# define MAVLINK_COMM_NUM_BUFFERS 16 -#else -# define MAVLINK_COMM_NUM_BUFFERS 4 -#endif -#endif - typedef enum { MAVLINK_PARSE_STATE_UNINIT=0, MAVLINK_PARSE_STATE_IDLE, diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 45a5e9566..7cfe0d9d0 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "minimal.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 0e5c4db5c..fea5381e9 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE HEARTBEAT PACKING #define MAVLINK_MSG_ID_HEARTBEAT 0 +#define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 +#define MAVLINK_MSG_0_LEN 3 typedef struct __mavlink_heartbeat_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_heartbeat_t } mavlink_heartbeat_t; - - /** * @brief Pack a heartbeat message * @param system_id ID of this system @@ -24,14 +24,14 @@ typedef struct __mavlink_heartbeat_t */ 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; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; 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 + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message(msg, system_id, component_id, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -46,14 +46,14 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com */ 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; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; 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 + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } /** @@ -77,12 +77,61 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_message_t msg; - mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) +{ + mavlink_header_t hdr; + mavlink_heartbeat_t payload; + uint16_t checksum; + mavlink_heartbeat_t *p = &payload; + + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -95,7 +144,8 @@ 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]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -105,7 +155,8 @@ 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]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->autopilot); } /** @@ -115,7 +166,8 @@ 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]; + mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; + return (uint8_t)(p->mavlink_version); } /** @@ -126,7 +178,5 @@ 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) { - 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); + memcpy( heartbeat, msg->payload, sizeof(mavlink_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index 61cd3fe22..47a266ccb 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -32,13 +32,6 @@ extern "C" { // MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" - - -// MESSAGE LENGTHS - -#undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { } - #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index 16a37599e..963c3697a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "pixhawk.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index 1a8b806f8..25ed69410 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -1,6 +1,8 @@ // MESSAGE ATTITUDE_CONTROL PACKING #define MAVLINK_MSG_ID_ATTITUDE_CONTROL 85 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21 +#define MAVLINK_MSG_85_LEN 21 typedef struct __mavlink_attitude_control_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_attitude_control_t } mavlink_attitude_control_t; - - /** * @brief Pack a attitude_control message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_attitude_control_t */ static inline uint16_t mavlink_msg_attitude_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; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_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 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_attitude_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; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_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 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t: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, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui * @param thrust_manual thrust auto:0, manual:1 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_attitude_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_attitude_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); + uint16_t checksum; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_attitude_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_header_t hdr; + mavlink_attitude_control_t payload; + uint16_t checksum; + mavlink_attitude_control_t *p = &payload; + + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,7 +195,8 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -144,12 +206,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_target(const mavlink_mess */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -159,12 +217,8 @@ static inline float mavlink_msg_attitude_control_get_roll(const mavlink_message_ */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -174,12 +228,8 @@ static inline float mavlink_msg_attitude_control_get_pitch(const mavlink_message */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -189,12 +239,8 @@ static inline float mavlink_msg_attitude_control_get_yaw(const mavlink_message_t */ static inline float mavlink_msg_attitude_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; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (float)(p->thrust); } /** @@ -204,7 +250,8 @@ static inline float mavlink_msg_attitude_control_get_thrust(const mavlink_messag */ static inline uint8_t mavlink_msg_attitude_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]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->roll_manual); } /** @@ -214,7 +261,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_roll_manual(const mavlink */ static inline uint8_t mavlink_msg_attitude_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]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->pitch_manual); } /** @@ -224,7 +272,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_pitch_manual(const mavlin */ static inline uint8_t mavlink_msg_attitude_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]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->yaw_manual); } /** @@ -234,7 +283,8 @@ static inline uint8_t mavlink_msg_attitude_control_get_yaw_manual(const mavlink_ */ static inline uint8_t mavlink_msg_attitude_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]; + mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; + return (uint8_t)(p->thrust_manual); } /** @@ -245,13 +295,5 @@ static inline uint8_t mavlink_msg_attitude_control_get_thrust_manual(const mavli */ static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* msg, mavlink_attitude_control_t* attitude_control) { - attitude_control->target = mavlink_msg_attitude_control_get_target(msg); - attitude_control->roll = mavlink_msg_attitude_control_get_roll(msg); - attitude_control->pitch = mavlink_msg_attitude_control_get_pitch(msg); - attitude_control->yaw = mavlink_msg_attitude_control_get_yaw(msg); - attitude_control->thrust = mavlink_msg_attitude_control_get_thrust(msg); - attitude_control->roll_manual = mavlink_msg_attitude_control_get_roll_manual(msg); - attitude_control->pitch_manual = mavlink_msg_attitude_control_get_pitch_manual(msg); - attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg); - attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg); + memcpy( attitude_control, msg->payload, sizeof(mavlink_attitude_control_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index f9dbde8c8..9cb8641e8 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -1,6 +1,8 @@ // MESSAGE AUX_STATUS PACKING #define MAVLINK_MSG_ID_AUX_STATUS 142 +#define MAVLINK_MSG_ID_AUX_STATUS_LEN 12 +#define MAVLINK_MSG_142_LEN 12 typedef struct __mavlink_aux_status_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_aux_status_t } mavlink_aux_status_t; - - /** * @brief Pack a aux_status message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_aux_status_t */ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - uint16_t i = 0; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - 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(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUX_STATUS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_aux_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { - uint16_t i = 0; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - 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(i2c0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(i2c1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi0_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(spi1_err_count, i, msg->payload); // Number of I2C errors since startup - i += put_uint16_t_by_index(uart_total_err_count, i, msg->payload); // Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUX_STATUS_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param uart_total_err_count Number of I2C errors since startup */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { mavlink_message_t msg; - mavlink_msg_aux_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, load, i2c0_err_count, i2c1_err_count, spi0_err_count, spi1_err_count, uart_total_err_count); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg.payload[0]; + + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_AUX_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) +{ + mavlink_header_t hdr; + mavlink_aux_status_t payload; + uint16_t checksum; + mavlink_aux_status_t *p = &payload; + + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_AUX_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,10 +171,8 @@ static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t */ static inline uint16_t mavlink_msg_aux_status_get_load(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; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->load); } /** @@ -129,10 +182,8 @@ static inline uint16_t mavlink_msg_aux_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(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; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->i2c0_err_count); } /** @@ -142,10 +193,8 @@ static inline uint16_t mavlink_msg_aux_status_get_i2c0_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(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; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->i2c1_err_count); } /** @@ -155,10 +204,8 @@ static inline uint16_t mavlink_msg_aux_status_get_i2c1_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(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; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->spi0_err_count); } /** @@ -168,10 +215,8 @@ static inline uint16_t mavlink_msg_aux_status_get_spi0_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(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; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->spi1_err_count); } /** @@ -181,10 +226,8 @@ static inline uint16_t mavlink_msg_aux_status_get_spi1_err_count(const mavlink_m */ static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(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; + mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; + return (uint16_t)(p->uart_total_err_count); } /** @@ -195,10 +238,5 @@ static inline uint16_t mavlink_msg_aux_status_get_uart_total_err_count(const mav */ static inline void mavlink_msg_aux_status_decode(const mavlink_message_t* msg, mavlink_aux_status_t* aux_status) { - aux_status->load = mavlink_msg_aux_status_get_load(msg); - aux_status->i2c0_err_count = mavlink_msg_aux_status_get_i2c0_err_count(msg); - aux_status->i2c1_err_count = mavlink_msg_aux_status_get_i2c1_err_count(msg); - aux_status->spi0_err_count = mavlink_msg_aux_status_get_spi0_err_count(msg); - aux_status->spi1_err_count = mavlink_msg_aux_status_get_spi1_err_count(msg); - aux_status->uart_total_err_count = mavlink_msg_aux_status_get_uart_total_err_count(msg); + memcpy( aux_status, msg->payload, sizeof(mavlink_aux_status_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index a61e13bcd..96814fcab 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -1,6 +1,8 @@ // MESSAGE BRIEF_FEATURE PACKING #define MAVLINK_MSG_ID_BRIEF_FEATURE 172 +#define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 +#define MAVLINK_MSG_172_LEN 53 typedef struct __mavlink_brief_feature_t { @@ -10,14 +12,12 @@ typedef struct __mavlink_brief_feature_t uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true uint16_t size; ///< Size in pixels uint16_t orientation; ///< Orientation - uint8_t descriptor[32]; ///< Descriptor + char descriptor[32]; ///< Descriptor float response; ///< Harris operator response at this location } mavlink_brief_feature_t; - #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 - /** * @brief Pack a brief_feature message * @param system_id ID of this system @@ -34,21 +34,21 @@ typedef struct __mavlink_brief_feature_t * @param response Harris operator response at this location * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) +static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) { - uint16_t i = 0; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } /** @@ -67,21 +67,21 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t * @param response Harris operator response at this location * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) +static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) { - uint16_t i = 0; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - i += put_float_by_index(x, i, msg->payload); // x position in m - i += put_float_by_index(y, i, msg->payload); // y position in m - i += put_float_by_index(z, i, msg->payload); // z position in m - i += put_uint8_t_by_index(orientation_assignment, i, msg->payload); // Orientation assignment 0: false, 1:true - i += put_uint16_t_by_index(size, i, msg->payload); // Size in pixels - i += put_uint16_t_by_index(orientation, i, msg->payload); // Orientation - i += put_array_by_index((const int8_t*)descriptor, sizeof(uint8_t)*32, i, msg->payload); // Descriptor - i += put_float_by_index(response, i, msg->payload); // Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } /** @@ -111,12 +111,71 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param response Harris operator response at this location */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) +static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) { mavlink_message_t msg; - mavlink_msg_brief_feature_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, x, y, z, orientation_assignment, size, orientation, descriptor, response); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg.payload[0]; + + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; + msg.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) +{ + mavlink_header_t hdr; + mavlink_brief_feature_t payload; + uint16_t checksum; + mavlink_brief_feature_t *p = &payload; + + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + p->response = response; // float:Harris operator response at this location + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; + hdr.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -129,12 +188,8 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float */ static inline float mavlink_msg_brief_feature_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; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -144,12 +199,8 @@ static inline float mavlink_msg_brief_feature_get_x(const mavlink_message_t* msg */ static inline float mavlink_msg_brief_feature_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; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -159,12 +210,8 @@ static inline float mavlink_msg_brief_feature_get_y(const mavlink_message_t* msg */ static inline float mavlink_msg_brief_feature_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; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -174,7 +221,8 @@ static inline float mavlink_msg_brief_feature_get_z(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint8_t)(p->orientation_assignment); } /** @@ -184,10 +232,8 @@ static inline uint8_t mavlink_msg_brief_feature_get_orientation_assignment(const */ static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint16_t)(p->size); } /** @@ -197,10 +243,8 @@ static inline uint16_t mavlink_msg_brief_feature_get_size(const mavlink_message_ */ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (uint16_t)(p->orientation); } /** @@ -208,11 +252,12 @@ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_m * * @return Descriptor */ -static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t* r_data) +static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, char* descriptor) { + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t), sizeof(uint8_t)*32); - return sizeof(uint8_t)*32; + memcpy(descriptor, p->descriptor, sizeof(p->descriptor)); + return sizeof(p->descriptor); } /** @@ -222,12 +267,8 @@ static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_me */ static inline float mavlink_msg_brief_feature_get_response(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[0]; - r.b[2] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[1]; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[2]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)*32)[3]; - return (float)r.f; + mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; + return (float)(p->response); } /** @@ -238,12 +279,5 @@ static inline float mavlink_msg_brief_feature_get_response(const mavlink_message */ static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg, mavlink_brief_feature_t* brief_feature) { - brief_feature->x = mavlink_msg_brief_feature_get_x(msg); - brief_feature->y = mavlink_msg_brief_feature_get_y(msg); - brief_feature->z = mavlink_msg_brief_feature_get_z(msg); - brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); - brief_feature->size = mavlink_msg_brief_feature_get_size(msg); - brief_feature->orientation = mavlink_msg_brief_feature_get_orientation(msg); - mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); - brief_feature->response = mavlink_msg_brief_feature_get_response(msg); + memcpy( brief_feature, msg->payload, sizeof(mavlink_brief_feature_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h index e9e69f7c9..22116a54c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -1,6 +1,8 @@ // MESSAGE DATA_TRANSMISSION_HANDSHAKE PACKING #define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 170 +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 8 +#define MAVLINK_MSG_170_LEN 8 typedef struct __mavlink_data_transmission_handshake_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_data_transmission_handshake_t } mavlink_data_transmission_handshake_t; - - /** * @brief Pack a data_transmission_handshake message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_data_transmission_handshake_t */ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - uint16_t i = 0; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst */ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { - uint16_t i = 0; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - i += put_uint8_t_by_index(type, i, msg->payload); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - i += put_uint32_t_by_index(size, i, msg->payload); // total data size in bytes (set on ACK only) - i += put_uint8_t_by_index(packets, i, msg->payload); // number of packets beeing sent (set on ACK only) - i += put_uint8_t_by_index(payload, i, msg->payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - i += put_uint8_t_by_index(jpg_quality, i, msg->payload); // JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy * @param jpg_quality JPEG quality out of [1,100] */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { mavlink_message_t msg; - mavlink_msg_data_transmission_handshake_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, size, packets, payload, jpg_quality); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg.payload[0]; + + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; + msg.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) +{ + mavlink_header_t hdr; + mavlink_data_transmission_handshake_t payload; + uint16_t checksum; + mavlink_data_transmission_handshake_t *p = &payload; + + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; + hdr.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,7 +163,8 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -120,12 +174,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_type(const mav */ static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(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; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint32_t)(p->size); } /** @@ -135,7 +185,8 @@ static inline uint32_t mavlink_msg_data_transmission_handshake_get_size(const ma */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->packets); } /** @@ -145,7 +196,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_packets(const */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->payload); } /** @@ -155,7 +207,8 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_payload(const */ static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; + return (uint8_t)(p->jpg_quality); } /** @@ -166,9 +219,5 @@ static inline uint8_t mavlink_msg_data_transmission_handshake_get_jpg_quality(co */ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_message_t* msg, mavlink_data_transmission_handshake_t* data_transmission_handshake) { - data_transmission_handshake->type = mavlink_msg_data_transmission_handshake_get_type(msg); - data_transmission_handshake->size = mavlink_msg_data_transmission_handshake_get_size(msg); - data_transmission_handshake->packets = mavlink_msg_data_transmission_handshake_get_packets(msg); - data_transmission_handshake->payload = mavlink_msg_data_transmission_handshake_get_payload(msg); - data_transmission_handshake->jpg_quality = mavlink_msg_data_transmission_handshake_get_jpg_quality(msg); + memcpy( data_transmission_handshake, msg->payload, sizeof(mavlink_data_transmission_handshake_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index c2c040ae6..cd052fcb4 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -1,6 +1,8 @@ // MESSAGE ENCAPSULATED_DATA PACKING #define MAVLINK_MSG_ID_ENCAPSULATED_DATA 171 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 +#define MAVLINK_MSG_171_LEN 255 typedef struct __mavlink_encapsulated_data_t { @@ -8,10 +10,8 @@ typedef struct __mavlink_encapsulated_data_t uint8_t data[253]; ///< image data bytes } mavlink_encapsulated_data_t; - #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 - /** * @brief Pack a encapsulated_data message * @param system_id ID of this system @@ -24,13 +24,13 @@ typedef struct __mavlink_encapsulated_data_t */ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) { - uint16_t i = 0; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } /** @@ -45,13 +45,13 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t seqnr, const uint8_t* data) { - uint16_t i = 0; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - i += put_uint16_t_by_index(seqnr, i, msg->payload); // sequence number (starting with 0 on every transmission) - i += put_array_by_index((const int8_t*)data, sizeof(uint8_t)*253, i, msg->payload); // image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } /** @@ -75,12 +75,59 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param data image data bytes */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) { mavlink_message_t msg; - mavlink_msg_encapsulated_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, seqnr, data); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg.payload[0]; + + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; + msg.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) +{ + mavlink_header_t hdr; + mavlink_encapsulated_data_t payload; + uint16_t checksum; + mavlink_encapsulated_data_t *p = &payload; + + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; + hdr.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -93,10 +140,8 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui */ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(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; + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; + return (uint16_t)(p->seqnr); } /** @@ -104,11 +149,12 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_mes * * @return image data bytes */ -static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* r_data) +static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t* data) { + mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t), sizeof(uint8_t)*253); - return sizeof(uint8_t)*253; + memcpy(data, p->data, sizeof(p->data)); + return sizeof(p->data); } /** @@ -119,6 +165,5 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_mess */ static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* msg, mavlink_encapsulated_data_t* encapsulated_data) { - encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); - mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); + memcpy( encapsulated_data, msg->payload, sizeof(mavlink_encapsulated_data_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index b40e23df7..49869a01e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_AVAILABLE PACKING #define MAVLINK_MSG_ID_IMAGE_AVAILABLE 103 +#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 +#define MAVLINK_MSG_103_LEN 92 typedef struct __mavlink_image_available_t { @@ -30,8 +32,6 @@ typedef struct __mavlink_image_available_t } mavlink_image_available_t; - - /** * @brief Pack a image_available message * @param system_id ID of this system @@ -65,34 +65,34 @@ typedef struct __mavlink_image_available_t */ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } /** @@ -128,34 +128,34 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - i += put_uint64_t_by_index(cam_id, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera # (starts with 0) - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint64_t_by_index(valid_until, i, msg->payload); // Until which timestamp this buffer will stay valid - i += put_uint32_t_by_index(img_seq, i, msg->payload); // The image sequence number - i += put_uint32_t_by_index(img_buf_index, i, msg->payload); // Position of the image in the buffer, starts with 0 - i += put_uint16_t_by_index(width, i, msg->payload); // Image width - i += put_uint16_t_by_index(height, i, msg->payload); // Image height - i += put_uint16_t_by_index(depth, i, msg->payload); // Image depth - i += put_uint8_t_by_index(channels, i, msg->payload); // Image channels - i += put_uint32_t_by_index(key, i, msg->payload); // Shared memory area key - i += put_uint32_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } /** @@ -200,12 +200,101 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin * @param ground_z Ground truth Z */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { mavlink_message_t msg; - mavlink_msg_image_available_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg.payload[0]; + + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; + msg.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) +{ + mavlink_header_t hdr; + mavlink_image_available_t payload; + uint16_t checksum; + mavlink_image_available_t *p = &payload; + + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -218,16 +307,8 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_image_available_get_cam_id(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; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->cam_id); } /** @@ -237,7 +318,8 @@ static inline uint64_t mavlink_msg_image_available_get_cam_id(const mavlink_mess */ static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t))[0]; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint8_t)(p->cam_no); } /** @@ -247,16 +329,8 @@ static inline uint8_t mavlink_msg_image_available_get_cam_no(const mavlink_messa */ static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->timestamp); } /** @@ -266,16 +340,8 @@ static inline uint64_t mavlink_msg_image_available_get_timestamp(const mavlink_m */ static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink_message_t* msg) { - generic_64bit r; - r.b[7] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[0]; - r.b[6] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[1]; - r.b[5] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[2]; - r.b[4] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[3]; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[4]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[5]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[6]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t))[7]; - return (uint64_t)r.ll; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint64_t)(p->valid_until); } /** @@ -285,12 +351,8 @@ static inline uint64_t mavlink_msg_image_available_get_valid_until(const mavlink */ static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->img_seq); } /** @@ -300,12 +362,8 @@ static inline uint32_t mavlink_msg_image_available_get_img_seq(const mavlink_mes */ static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->img_buf_index); } /** @@ -315,10 +373,8 @@ static inline uint32_t mavlink_msg_image_available_get_img_buf_index(const mavli */ static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->width); } /** @@ -328,10 +384,8 @@ static inline uint16_t mavlink_msg_image_available_get_width(const mavlink_messa */ static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->height); } /** @@ -341,10 +395,8 @@ static inline uint16_t mavlink_msg_image_available_get_height(const mavlink_mess */ static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint16_t)(p->depth); } /** @@ -354,7 +406,8 @@ static inline uint16_t mavlink_msg_image_available_get_depth(const mavlink_messa */ static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint8_t)(p->channels); } /** @@ -364,12 +417,8 @@ static inline uint8_t mavlink_msg_image_available_get_channels(const mavlink_mes */ static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->key); } /** @@ -379,12 +428,8 @@ static inline uint32_t mavlink_msg_image_available_get_key(const mavlink_message */ static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t))[3]; - return (uint32_t)r.i; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (uint32_t)(p->exposure); } /** @@ -394,12 +439,8 @@ static inline uint32_t mavlink_msg_image_available_get_exposure(const mavlink_me */ static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->gain); } /** @@ -409,12 +450,8 @@ static inline float mavlink_msg_image_available_get_gain(const mavlink_message_t */ static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -424,12 +461,8 @@ static inline float mavlink_msg_image_available_get_roll(const mavlink_message_t */ static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -439,12 +472,8 @@ static inline float mavlink_msg_image_available_get_pitch(const mavlink_message_ */ static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -454,12 +483,8 @@ static inline float mavlink_msg_image_available_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_local_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->local_z); } /** @@ -469,12 +494,8 @@ static inline float mavlink_msg_image_available_get_local_z(const mavlink_messag */ static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -484,12 +505,8 @@ static inline float mavlink_msg_image_available_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -499,12 +516,8 @@ static inline float mavlink_msg_image_available_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -514,12 +527,8 @@ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_x); } /** @@ -529,12 +538,8 @@ static inline float mavlink_msg_image_available_get_ground_x(const mavlink_messa */ static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_y); } /** @@ -544,12 +549,8 @@ static inline float mavlink_msg_image_available_get_ground_y(const mavlink_messa */ static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+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(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; + return (float)(p->ground_z); } /** @@ -560,27 +561,5 @@ static inline float mavlink_msg_image_available_get_ground_z(const mavlink_messa */ static inline void mavlink_msg_image_available_decode(const mavlink_message_t* msg, mavlink_image_available_t* image_available) { - image_available->cam_id = mavlink_msg_image_available_get_cam_id(msg); - image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg); - image_available->timestamp = mavlink_msg_image_available_get_timestamp(msg); - image_available->valid_until = mavlink_msg_image_available_get_valid_until(msg); - image_available->img_seq = mavlink_msg_image_available_get_img_seq(msg); - image_available->img_buf_index = mavlink_msg_image_available_get_img_buf_index(msg); - image_available->width = mavlink_msg_image_available_get_width(msg); - image_available->height = mavlink_msg_image_available_get_height(msg); - image_available->depth = mavlink_msg_image_available_get_depth(msg); - image_available->channels = mavlink_msg_image_available_get_channels(msg); - image_available->key = mavlink_msg_image_available_get_key(msg); - image_available->exposure = mavlink_msg_image_available_get_exposure(msg); - image_available->gain = mavlink_msg_image_available_get_gain(msg); - image_available->roll = mavlink_msg_image_available_get_roll(msg); - image_available->pitch = mavlink_msg_image_available_get_pitch(msg); - image_available->yaw = mavlink_msg_image_available_get_yaw(msg); - image_available->local_z = mavlink_msg_image_available_get_local_z(msg); - image_available->lat = mavlink_msg_image_available_get_lat(msg); - image_available->lon = mavlink_msg_image_available_get_lon(msg); - image_available->alt = mavlink_msg_image_available_get_alt(msg); - image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg); - image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg); - image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg); + memcpy( image_available, msg->payload, sizeof(mavlink_image_available_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h index 90aa9dcf4..0e81a6c6d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_TRIGGER_CONTROL PACKING #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 102 +#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 +#define MAVLINK_MSG_102_LEN 1 typedef struct __mavlink_image_trigger_control_t { @@ -8,8 +10,6 @@ typedef struct __mavlink_image_trigger_control_t } mavlink_image_trigger_control_t; - - /** * @brief Pack a image_trigger_control message * @param system_id ID of this system @@ -21,12 +21,12 @@ typedef struct __mavlink_image_trigger_control_t */ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enable) { - uint16_t i = 0; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } /** @@ -40,12 +40,12 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enable) { - uint16_t i = 0; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - i += put_uint8_t_by_index(enable, i, msg->payload); // 0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } /** @@ -68,12 +68,57 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i * @param enable 0 to disable, 1 to enable */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { mavlink_message_t msg; - mavlink_msg_image_trigger_control_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enable); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg.payload[0]; + + p->enable = enable; // uint8_t:0 to disable, 1 to enable + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; + msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) +{ + mavlink_header_t hdr; + mavlink_image_trigger_control_t payload; + uint16_t checksum; + mavlink_image_trigger_control_t *p = &payload; + + p->enable = enable; // uint8_t:0 to disable, 1 to enable + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -86,7 +131,8 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; + return (uint8_t)(p->enable); } /** @@ -97,5 +143,5 @@ static inline uint8_t mavlink_msg_image_trigger_control_get_enable(const mavlink */ static inline void mavlink_msg_image_trigger_control_decode(const mavlink_message_t* msg, mavlink_image_trigger_control_t* image_trigger_control) { - image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); + memcpy( image_trigger_control, msg->payload, sizeof(mavlink_image_trigger_control_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 000003f3d..6087d3523 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -1,6 +1,8 @@ // MESSAGE IMAGE_TRIGGERED PACKING #define MAVLINK_MSG_ID_IMAGE_TRIGGERED 101 +#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 +#define MAVLINK_MSG_101_LEN 52 typedef struct __mavlink_image_triggered_t { @@ -19,8 +21,6 @@ typedef struct __mavlink_image_triggered_t } mavlink_image_triggered_t; - - /** * @brief Pack a image_triggered message * @param system_id ID of this system @@ -43,23 +43,23 @@ typedef struct __mavlink_image_triggered_t */ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); } /** @@ -84,23 +84,23 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { - uint16_t i = 0; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - i += put_uint64_t_by_index(timestamp, i, msg->payload); // Timestamp - i += put_uint32_t_by_index(seq, i, msg->payload); // IMU seq - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad - i += put_float_by_index(local_z, i, msg->payload); // Local frame Z coordinate (height over ground) - i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate - i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate - i += put_float_by_index(alt, i, msg->payload); // Global frame altitude - i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X - i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y - i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); } /** @@ -134,12 +134,79 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin * @param ground_z Ground truth Z */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { mavlink_message_t msg; - mavlink_msg_image_triggered_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg.payload[0]; + + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; + msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) +{ + mavlink_header_t hdr; + mavlink_image_triggered_t payload; + uint16_t checksum; + mavlink_image_triggered_t *p = &payload; + + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; + hdr.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -152,16 +219,8 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_image_triggered_get_timestamp(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; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (uint64_t)(p->timestamp); } /** @@ -171,12 +230,8 @@ static inline uint64_t mavlink_msg_image_triggered_get_timestamp(const mavlink_m */ static inline uint32_t mavlink_msg_image_triggered_get_seq(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 (uint32_t)r.i; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (uint32_t)(p->seq); } /** @@ -186,12 +241,8 @@ static inline uint32_t mavlink_msg_image_triggered_get_seq(const mavlink_message */ static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +252,8 @@ static inline float mavlink_msg_image_triggered_get_roll(const mavlink_message_t */ static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +263,8 @@ static inline float mavlink_msg_image_triggered_get_pitch(const mavlink_message_ */ static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -231,12 +274,8 @@ static inline float mavlink_msg_image_triggered_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->local_z); } /** @@ -246,12 +285,8 @@ static inline float mavlink_msg_image_triggered_get_local_z(const mavlink_messag */ static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->lat); } /** @@ -261,12 +296,8 @@ static inline float mavlink_msg_image_triggered_get_lat(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->lon); } /** @@ -276,12 +307,8 @@ static inline float mavlink_msg_image_triggered_get_lon(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->alt); } /** @@ -291,12 +318,8 @@ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t* */ static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_x); } /** @@ -306,12 +329,8 @@ static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_messa */ static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_y); } /** @@ -321,12 +340,8 @@ static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_messa */ static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; + return (float)(p->ground_z); } /** @@ -337,16 +352,5 @@ static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_messa */ static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* msg, mavlink_image_triggered_t* image_triggered) { - image_triggered->timestamp = mavlink_msg_image_triggered_get_timestamp(msg); - image_triggered->seq = mavlink_msg_image_triggered_get_seq(msg); - image_triggered->roll = mavlink_msg_image_triggered_get_roll(msg); - image_triggered->pitch = mavlink_msg_image_triggered_get_pitch(msg); - image_triggered->yaw = mavlink_msg_image_triggered_get_yaw(msg); - image_triggered->local_z = mavlink_msg_image_triggered_get_local_z(msg); - image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg); - image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg); - image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg); - image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg); - image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg); - image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg); + memcpy( image_triggered, msg->payload, sizeof(mavlink_image_triggered_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index fb275534a..e5519ef6d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -1,6 +1,8 @@ // MESSAGE MARKER PACKING #define MAVLINK_MSG_ID_MARKER 130 +#define MAVLINK_MSG_ID_MARKER_LEN 26 +#define MAVLINK_MSG_130_LEN 26 typedef struct __mavlink_marker_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_marker_t } mavlink_marker_t; - - /** * @brief Pack a marker message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_marker_t */ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - i += put_uint16_t_by_index(id, i, msg->payload); // 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(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon */ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - i += put_uint16_t_by_index(id, i, msg->payload); // 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(roll, i, msg->payload); // roll orientation - i += put_float_by_index(pitch, i, msg->payload); // pitch orientation - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp * @param yaw yaw orientation */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_message_t msg; - mavlink_msg_marker_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_marker_t *p = (mavlink_marker_t *)&msg.payload[0]; + + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_MARKER_LEN; + msg.msgid = MAVLINK_MSG_ID_MARKER; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_marker_t payload; + uint16_t checksum; + mavlink_marker_t *p = &payload; + + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MARKER_LEN; + hdr.msgid = MAVLINK_MSG_ID_MARKER; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,10 +179,8 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, */ static inline uint16_t mavlink_msg_marker_get_id(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; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -135,12 +190,8 @@ static inline uint16_t mavlink_msg_marker_get_id(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -150,12 +201,8 @@ static inline float mavlink_msg_marker_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -165,12 +212,8 @@ static inline float mavlink_msg_marker_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -180,12 +223,8 @@ static inline float mavlink_msg_marker_get_z(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -195,12 +234,8 @@ static inline float mavlink_msg_marker_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -210,12 +245,8 @@ static inline float mavlink_msg_marker_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -226,11 +257,5 @@ static inline float mavlink_msg_marker_get_yaw(const mavlink_message_t* msg) */ static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavlink_marker_t* marker) { - marker->id = mavlink_msg_marker_get_id(msg); - marker->x = mavlink_msg_marker_get_x(msg); - marker->y = mavlink_msg_marker_get_y(msg); - marker->z = mavlink_msg_marker_get_z(msg); - marker->roll = mavlink_msg_marker_get_roll(msg); - marker->pitch = mavlink_msg_marker_get_pitch(msg); - marker->yaw = mavlink_msg_marker_get_yaw(msg); + memcpy( marker, msg->payload, sizeof(mavlink_marker_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 1a2b46596..311e3be97 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -1,19 +1,19 @@ // MESSAGE PATTERN_DETECTED PACKING #define MAVLINK_MSG_ID_PATTERN_DETECTED 160 +#define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106 +#define MAVLINK_MSG_160_LEN 106 typedef struct __mavlink_pattern_detected_t { uint8_t type; ///< 0: Pattern, 1: Letter float confidence; ///< Confidence of detection - int8_t file[100]; ///< Pattern file name + char file[100]; ///< Pattern file name uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; - #define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 - /** * @brief Pack a pattern_detected message * @param system_id ID of this system @@ -26,17 +26,17 @@ typedef struct __mavlink_pattern_detected_t * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { - uint16_t i = 0; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } /** @@ -51,17 +51,17 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) { - uint16_t i = 0; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Pattern, 1: Letter - i += put_float_by_index(confidence, i, msg->payload); // Confidence of detection - i += put_array_by_index(file, 100, i, msg->payload); // Pattern file name - i += put_uint8_t_by_index(detected, i, msg->payload); // Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } /** @@ -87,12 +87,63 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param detected Accepted as true detection, 0 no, 1 yes */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const int8_t* file, uint8_t detected) +static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) { mavlink_message_t msg; - mavlink_msg_pattern_detected_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, confidence, file, detected); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg.payload[0]; + + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; + msg.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) +{ + mavlink_header_t hdr; + mavlink_pattern_detected_t payload; + uint16_t checksum; + mavlink_pattern_detected_t *p = &payload; + + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; + hdr.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -105,7 +156,8 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -115,12 +167,8 @@ static inline uint8_t mavlink_msg_pattern_detected_get_type(const mavlink_messag */ static inline float mavlink_msg_pattern_detected_get_confidence(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; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (float)(p->confidence); } /** @@ -128,11 +176,12 @@ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_me * * @return Pattern file name */ -static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char* file) { + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(float), 100); - return 100; + memcpy(file, p->file, sizeof(p->file)); + return sizeof(p->file); } /** @@ -142,7 +191,8 @@ static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_messa */ static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(float)+100)[0]; + mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; + return (uint8_t)(p->detected); } /** @@ -153,8 +203,5 @@ static inline uint8_t mavlink_msg_pattern_detected_get_detected(const mavlink_me */ static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* msg, mavlink_pattern_detected_t* pattern_detected) { - pattern_detected->type = mavlink_msg_pattern_detected_get_type(msg); - pattern_detected->confidence = mavlink_msg_pattern_detected_get_confidence(msg); - mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); - pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); + memcpy( pattern_detected, msg->payload, sizeof(mavlink_pattern_detected_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h index 7e48c0fd9..284bd4a21 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -1,6 +1,8 @@ // MESSAGE POINT_OF_INTEREST PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 42 +#define MAVLINK_MSG_161_LEN 42 typedef struct __mavlink_point_of_interest_t { @@ -11,13 +13,11 @@ typedef struct __mavlink_point_of_interest_t float x; ///< X Position float y; ///< Y Position float z; ///< Z Position - int8_t name[25]; ///< POI name + char name[25]; ///< POI name } mavlink_point_of_interest_t; - #define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 25 - /** * @brief Pack a point_of_interest message * @param system_id ID of this system @@ -34,21 +34,21 @@ typedef struct __mavlink_point_of_interest_t * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - 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_array_by_index(name, 25, i, msg->payload); // POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } /** @@ -67,21 +67,21 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - 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_array_by_index(name, 25, i, msg->payload); // POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } /** @@ -111,12 +111,71 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param name POI name */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const int8_t* name) +static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { mavlink_message_t msg; - mavlink_msg_point_of_interest_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, x, y, z, name); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg.payload[0]; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; + msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) +{ + mavlink_header_t hdr; + mavlink_point_of_interest_t payload; + uint16_t checksum; + mavlink_point_of_interest_t *p = &payload; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; + hdr.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -129,7 +188,8 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui */ static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -139,7 +199,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_type(const mavlink_messa */ static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->color); } /** @@ -149,7 +210,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_color(const mavlink_mess */ static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint8_t)(p->coordinate_system); } /** @@ -159,10 +221,8 @@ static inline uint8_t mavlink_msg_point_of_interest_get_coordinate_system(const */ static inline uint16_t mavlink_msg_point_of_interest_get_timeout(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; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (uint16_t)(p->timeout); } /** @@ -172,12 +232,8 @@ static inline uint16_t mavlink_msg_point_of_interest_get_timeout(const mavlink_m */ static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -187,12 +243,8 @@ static inline float mavlink_msg_point_of_interest_get_x(const mavlink_message_t* */ static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -202,12 +254,8 @@ static inline float mavlink_msg_point_of_interest_get_y(const mavlink_message_t* */ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -215,11 +263,12 @@ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* * * @return POI name */ -static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char* name) { + mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -230,12 +279,5 @@ static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_mess */ static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* msg, mavlink_point_of_interest_t* point_of_interest) { - point_of_interest->type = mavlink_msg_point_of_interest_get_type(msg); - point_of_interest->color = mavlink_msg_point_of_interest_get_color(msg); - point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg); - point_of_interest->timeout = mavlink_msg_point_of_interest_get_timeout(msg); - point_of_interest->x = mavlink_msg_point_of_interest_get_x(msg); - point_of_interest->y = mavlink_msg_point_of_interest_get_y(msg); - point_of_interest->z = mavlink_msg_point_of_interest_get_z(msg); - mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); + memcpy( point_of_interest, msg->payload, sizeof(mavlink_point_of_interest_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h index fb3051746..972450692 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -1,6 +1,8 @@ // MESSAGE POINT_OF_INTEREST_CONNECTION PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 54 +#define MAVLINK_MSG_162_LEN 54 typedef struct __mavlink_point_of_interest_connection_t { @@ -14,13 +16,11 @@ typedef struct __mavlink_point_of_interest_connection_t float xp2; ///< X2 Position float yp2; ///< Y2 Position float zp2; ///< Z2 Position - int8_t name[25]; ///< POI connection name + char name[25]; ///< POI connection name } mavlink_point_of_interest_connection_t; - #define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 25 - /** * @brief Pack a point_of_interest_connection message * @param system_id ID of this system @@ -40,24 +40,24 @@ typedef struct __mavlink_point_of_interest_connection_t * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name - - return mavlink_finalize_message(msg, system_id, component_id, i); + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } /** @@ -79,24 +79,24 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { - uint16_t i = 0; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - i += put_uint8_t_by_index(type, i, msg->payload); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - i += put_uint8_t_by_index(color, i, msg->payload); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - i += put_uint8_t_by_index(coordinate_system, i, msg->payload); // 0: global, 1:local - i += put_uint16_t_by_index(timeout, i, msg->payload); // 0: no timeout, >1: timeout in seconds - i += put_float_by_index(xp1, i, msg->payload); // X1 Position - i += put_float_by_index(yp1, i, msg->payload); // Y1 Position - i += put_float_by_index(zp1, i, msg->payload); // Z1 Position - i += put_float_by_index(xp2, i, msg->payload); // X2 Position - i += put_float_by_index(yp2, i, msg->payload); // Y2 Position - i += put_float_by_index(zp2, i, msg->payload); // Z2 Position - i += put_array_by_index(name, 25, i, msg->payload); // POI connection name - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } /** @@ -129,12 +129,77 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param name POI connection name */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const int8_t* name) +static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { mavlink_message_t msg; - mavlink_msg_point_of_interest_connection_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, color, coordinate_system, timeout, xp1, yp1, zp1, xp2, yp2, zp2, name); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg.payload[0]; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; + msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) +{ + mavlink_header_t hdr; + mavlink_point_of_interest_connection_t payload; + uint16_t checksum; + mavlink_point_of_interest_connection_t *p = &payload; + + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -147,7 +212,8 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->type); } /** @@ -157,7 +223,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_type(const ma */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->color); } /** @@ -167,7 +234,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_color(const m */ static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint8_t)(p->coordinate_system); } /** @@ -177,10 +245,8 @@ static inline uint8_t mavlink_msg_point_of_interest_connection_get_coordinate_sy */ static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(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; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (uint16_t)(p->timeout); } /** @@ -190,12 +256,8 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_get_timeout(cons */ static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->xp1); } /** @@ -205,12 +267,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_xp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->yp1); } /** @@ -220,12 +278,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_yp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->zp1); } /** @@ -235,12 +289,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp1(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->xp2); } /** @@ -250,12 +300,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_xp2(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_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(uint16_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(uint16_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(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->yp2); } /** @@ -265,12 +311,8 @@ static inline float mavlink_msg_point_of_interest_connection_get_yp2(const mavli */ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_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(uint16_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(uint16_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(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; + return (float)(p->zp2); } /** @@ -278,11 +320,12 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavli * * @return POI connection name */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char* name) { + mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float), 25); - return 25; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -293,15 +336,5 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const m */ static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink_message_t* msg, mavlink_point_of_interest_connection_t* point_of_interest_connection) { - point_of_interest_connection->type = mavlink_msg_point_of_interest_connection_get_type(msg); - point_of_interest_connection->color = mavlink_msg_point_of_interest_connection_get_color(msg); - point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg); - point_of_interest_connection->timeout = mavlink_msg_point_of_interest_connection_get_timeout(msg); - point_of_interest_connection->xp1 = mavlink_msg_point_of_interest_connection_get_xp1(msg); - point_of_interest_connection->yp1 = mavlink_msg_point_of_interest_connection_get_yp1(msg); - point_of_interest_connection->zp1 = mavlink_msg_point_of_interest_connection_get_zp1(msg); - point_of_interest_connection->xp2 = mavlink_msg_point_of_interest_connection_get_xp2(msg); - point_of_interest_connection->yp2 = mavlink_msg_point_of_interest_connection_get_yp2(msg); - point_of_interest_connection->zp2 = mavlink_msg_point_of_interest_connection_get_zp2(msg); - mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); + memcpy( point_of_interest_connection, msg->payload, sizeof(mavlink_point_of_interest_connection_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h index 30f076bba..2b508a62a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROL_OFFSET_SET PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 154 +#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN 18 +#define MAVLINK_MSG_154_LEN 18 typedef struct __mavlink_position_control_offset_set_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_position_control_offset_set_t } mavlink_position_control_offset_set_t; - - /** * @brief Pack a position_control_offset_set message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_position_control_offset_set_t */ static inline uint16_t mavlink_msg_position_control_offset_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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_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 offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t syst */ static inline uint16_t mavlink_msg_position_control_offset_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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_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 offset - i += put_float_by_index(y, i, msg->payload); // y position offset - i += put_float_by_index(z, i, msg->payload); // z position offset - i += put_float_by_index(yaw, i, msg->payload); // yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset 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, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy * @param yaw yaw orientation offset in radians, 0 = NORTH */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_position_control_offset_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_position_control_offset_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) +{ + mavlink_header_t hdr; + mavlink_position_control_offset_set_t payload; + uint16_t checksum; + mavlink_position_control_offset_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +171,8 @@ static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -126,7 +182,8 @@ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_system( */ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -136,12 +193,8 @@ static inline uint8_t mavlink_msg_position_control_offset_set_get_target_compone */ static inline float mavlink_msg_position_control_offset_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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -151,12 +204,8 @@ static inline float mavlink_msg_position_control_offset_set_get_x(const mavlink_ */ static inline float mavlink_msg_position_control_offset_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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -166,12 +215,8 @@ static inline float mavlink_msg_position_control_offset_set_get_y(const mavlink_ */ static inline float mavlink_msg_position_control_offset_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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -181,12 +226,8 @@ static inline float mavlink_msg_position_control_offset_set_get_z(const mavlink_ */ static inline float mavlink_msg_position_control_offset_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; + mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -197,10 +238,5 @@ static inline float mavlink_msg_position_control_offset_set_get_yaw(const mavlin */ static inline void mavlink_msg_position_control_offset_set_decode(const mavlink_message_t* msg, mavlink_position_control_offset_set_t* position_control_offset_set) { - position_control_offset_set->target_system = mavlink_msg_position_control_offset_set_get_target_system(msg); - position_control_offset_set->target_component = mavlink_msg_position_control_offset_set_get_target_component(msg); - position_control_offset_set->x = mavlink_msg_position_control_offset_set_get_x(msg); - position_control_offset_set->y = mavlink_msg_position_control_offset_set_get_y(msg); - position_control_offset_set->z = mavlink_msg_position_control_offset_set_get_z(msg); - position_control_offset_set->yaw = mavlink_msg_position_control_offset_set_get_yaw(msg); + memcpy( position_control_offset_set, msg->payload, sizeof(mavlink_position_control_offset_set_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h index 59bcb131c..ee33580eb 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROL_SETPOINT PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 121 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 +#define MAVLINK_MSG_121_LEN 18 typedef struct __mavlink_position_control_setpoint_t { @@ -12,8 +14,6 @@ typedef struct __mavlink_position_control_setpoint_t } mavlink_position_control_setpoint_t; - - /** * @brief Pack a position_control_setpoint message * @param system_id ID of this system @@ -29,16 +29,16 @@ typedef struct __mavlink_position_control_setpoint_t */ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - 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 + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } /** @@ -56,16 +56,16 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system */ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - i += put_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - 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 + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } /** @@ -92,12 +92,65 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst * @param yaw yaw orientation in radians, 0 = NORTH */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) { mavlink_message_t msg; - mavlink_msg_position_control_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg.payload[0]; + + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) +{ + mavlink_header_t hdr; + mavlink_position_control_setpoint_t payload; + uint16_t checksum; + mavlink_position_control_setpoint_t *p = &payload; + + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -110,10 +163,8 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t */ static inline uint16_t mavlink_msg_position_control_setpoint_get_id(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; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -123,12 +174,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_get_id(const mavlin */ static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -138,12 +185,8 @@ static inline float mavlink_msg_position_control_setpoint_get_x(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -153,12 +196,8 @@ static inline float mavlink_msg_position_control_setpoint_get_y(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -168,12 +207,8 @@ static inline float mavlink_msg_position_control_setpoint_get_z(const mavlink_me */ static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -184,9 +219,5 @@ static inline float mavlink_msg_position_control_setpoint_get_yaw(const mavlink_ */ static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_t* position_control_setpoint) { - position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); - position_control_setpoint->x = mavlink_msg_position_control_setpoint_get_x(msg); - position_control_setpoint->y = mavlink_msg_position_control_setpoint_get_y(msg); - position_control_setpoint->z = mavlink_msg_position_control_setpoint_get_z(msg); - position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg); + memcpy( position_control_setpoint, msg->payload, sizeof(mavlink_position_control_setpoint_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h index 75150e66b..031a73b01 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h @@ -1,6 +1,8 @@ // MESSAGE POSITION_CONTROL_SETPOINT_SET PACKING #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 120 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN 20 +#define MAVLINK_MSG_120_LEN 20 typedef struct __mavlink_position_control_setpoint_set_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_position_control_setpoint_set_t } mavlink_position_control_setpoint_set_t; - - /** * @brief Pack a position_control_setpoint_set message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_position_control_setpoint_set_t */ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_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_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t sy */ static inline uint16_t mavlink_msg_position_control_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, uint16_t id, float x, float y, float z, float yaw) { - uint16_t i = 0; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_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_uint16_t_by_index(id, i, msg->payload); // ID of waypoint, 0 for plain position - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float: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, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t * @param yaw yaw orientation in radians, 0 = NORTH */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { mavlink_message_t msg; - mavlink_msg_position_control_setpoint_set_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, id, x, y, z, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg.payload[0]; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; + msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) +{ + mavlink_header_t hdr; + mavlink_position_control_setpoint_set_t payload; + uint16_t checksum; + mavlink_position_control_setpoint_set_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; + hdr.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channe */ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_system(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); } /** @@ -132,7 +190,8 @@ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_syste */ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_component(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); } /** @@ -142,10 +201,8 @@ static inline uint8_t mavlink_msg_position_control_setpoint_set_get_target_compo */ static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(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; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (uint16_t)(p->id); } /** @@ -155,12 +212,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_get_id(const ma */ static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -170,12 +223,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_x(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_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(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -185,12 +234,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_y(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_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(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -200,12 +245,8 @@ static inline float mavlink_msg_position_control_setpoint_set_get_z(const mavlin */ static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(float)+sizeof(float)+sizeof(float))[3]; - return (float)r.f; + mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -216,11 +257,5 @@ static inline float mavlink_msg_position_control_setpoint_set_get_yaw(const mavl */ static inline void mavlink_msg_position_control_setpoint_set_decode(const mavlink_message_t* msg, mavlink_position_control_setpoint_set_t* position_control_setpoint_set) { - position_control_setpoint_set->target_system = mavlink_msg_position_control_setpoint_set_get_target_system(msg); - position_control_setpoint_set->target_component = mavlink_msg_position_control_setpoint_set_get_target_component(msg); - position_control_setpoint_set->id = mavlink_msg_position_control_setpoint_set_get_id(msg); - position_control_setpoint_set->x = mavlink_msg_position_control_setpoint_set_get_x(msg); - position_control_setpoint_set->y = mavlink_msg_position_control_setpoint_set_get_y(msg); - position_control_setpoint_set->z = mavlink_msg_position_control_setpoint_set_get_z(msg); - position_control_setpoint_set->yaw = mavlink_msg_position_control_setpoint_set_get_yaw(msg); + memcpy( position_control_setpoint_set, msg->payload, sizeof(mavlink_position_control_setpoint_set_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 61adee297..32b82e568 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -1,6 +1,8 @@ // MESSAGE RAW_AUX PACKING #define MAVLINK_MSG_ID_RAW_AUX 141 +#define MAVLINK_MSG_ID_RAW_AUX_LEN 16 +#define MAVLINK_MSG_141_LEN 16 typedef struct __mavlink_raw_aux_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_raw_aux_t } mavlink_raw_aux_t; - - /** * @brief Pack a raw_aux message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_raw_aux_t */ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - uint16_t i = 0; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo */ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { - uint16_t i = 0; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - i += put_uint16_t_by_index(adc1, i, msg->payload); // ADC1 (J405 ADC3, LPC2148 AD0.6) - i += put_uint16_t_by_index(adc2, i, msg->payload); // ADC2 (J405 ADC5, LPC2148 AD0.2) - i += put_uint16_t_by_index(adc3, i, msg->payload); // ADC3 (J405 ADC6, LPC2148 AD0.1) - i += put_uint16_t_by_index(adc4, i, msg->payload); // ADC4 (J405 ADC7, LPC2148 AD1.3) - i += put_uint16_t_by_index(vbat, i, msg->payload); // Battery voltage - i += put_int16_t_by_index(temp, i, msg->payload); // Temperature (degrees celcius) - i += put_int32_t_by_index(baro, i, msg->payload); // Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com * @param baro Barometric pressure (hecto Pascal) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { mavlink_message_t msg; - mavlink_msg_raw_aux_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, adc1, adc2, adc3, adc4, vbat, temp, baro); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg.payload[0]; + + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RAW_AUX_LEN; + msg.msgid = MAVLINK_MSG_ID_RAW_AUX; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) +{ + mavlink_header_t hdr; + mavlink_raw_aux_t payload; + uint16_t checksum; + mavlink_raw_aux_t *p = &payload; + + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RAW_AUX_LEN; + hdr.msgid = MAVLINK_MSG_ID_RAW_AUX; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,10 +179,8 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc */ static inline uint16_t mavlink_msg_raw_aux_get_adc1(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; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc1); } /** @@ -135,10 +190,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc1(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc2(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; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc2); } /** @@ -148,10 +201,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc2(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc3(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; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc3); } /** @@ -161,10 +212,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc3(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_adc4(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; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->adc4); } /** @@ -174,10 +223,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_adc4(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_raw_aux_get_vbat(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; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (uint16_t)(p->vbat); } /** @@ -187,10 +234,8 @@ static inline uint16_t mavlink_msg_raw_aux_get_vbat(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_aux_get_temp(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 (int16_t)r.s; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (int16_t)(p->temp); } /** @@ -200,12 +245,8 @@ static inline int16_t mavlink_msg_raw_aux_get_temp(const mavlink_message_t* msg) */ static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(int16_t))[3]; - return (int32_t)r.i; + mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; + return (int32_t)(p->baro); } /** @@ -216,11 +257,5 @@ static inline int32_t mavlink_msg_raw_aux_get_baro(const mavlink_message_t* msg) */ static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavlink_raw_aux_t* raw_aux) { - raw_aux->adc1 = mavlink_msg_raw_aux_get_adc1(msg); - raw_aux->adc2 = mavlink_msg_raw_aux_get_adc2(msg); - raw_aux->adc3 = mavlink_msg_raw_aux_get_adc3(msg); - raw_aux->adc4 = mavlink_msg_raw_aux_get_adc4(msg); - raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg); - raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg); - raw_aux->baro = mavlink_msg_raw_aux_get_baro(msg); + memcpy( raw_aux, msg->payload, sizeof(mavlink_raw_aux_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h index 6a34b7e4b..5db1aa478 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -1,6 +1,8 @@ // MESSAGE SET_CAM_SHUTTER PACKING #define MAVLINK_MSG_ID_SET_CAM_SHUTTER 100 +#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 +#define MAVLINK_MSG_100_LEN 11 typedef struct __mavlink_set_cam_shutter_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_set_cam_shutter_t } mavlink_set_cam_shutter_t; - - /** * @brief Pack a set_cam_shutter message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_set_cam_shutter_t */ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - uint16_t i = 0; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { - uint16_t i = 0; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - i += put_uint8_t_by_index(cam_no, i, msg->payload); // Camera id - i += put_uint8_t_by_index(cam_mode, i, msg->payload); // Camera mode: 0 = auto, 1 = manual - i += put_uint8_t_by_index(trigger_pin, i, msg->payload); // Trigger pin, 0-3 for PtGrey FireFly - i += put_uint16_t_by_index(interval, i, msg->payload); // Shutter interval, in microseconds - i += put_uint16_t_by_index(exposure, i, msg->payload); // Exposure time, in microseconds - i += put_float_by_index(gain, i, msg->payload); // Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin * @param gain Camera gain */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { mavlink_message_t msg; - mavlink_msg_set_cam_shutter_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_no, cam_mode, trigger_pin, interval, exposure, gain); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg.payload[0]; + + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; + msg.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) +{ + mavlink_header_t hdr; + mavlink_set_cam_shutter_t payload; + uint16_t checksum; + mavlink_set_cam_shutter_t *p = &payload; + + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,7 +171,8 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->cam_no); } /** @@ -126,7 +182,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_no(const mavlink_messa */ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->cam_mode); } /** @@ -136,7 +193,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_cam_mode(const mavlink_mes */ static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint8_t)(p->trigger_pin); } /** @@ -146,10 +204,8 @@ static inline uint8_t mavlink_msg_set_cam_shutter_get_trigger_pin(const mavlink_ */ static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(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; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint16_t)(p->interval); } /** @@ -159,10 +215,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_get_interval(const mavlink_me */ static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(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; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (uint16_t)(p->exposure); } /** @@ -172,12 +226,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_get_exposure(const mavlink_me */ static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; - r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1]; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[2]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[3]; - return (float)r.f; + mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; + return (float)(p->gain); } /** @@ -188,10 +238,5 @@ static inline float mavlink_msg_set_cam_shutter_get_gain(const mavlink_message_t */ static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* msg, mavlink_set_cam_shutter_t* set_cam_shutter) { - set_cam_shutter->cam_no = mavlink_msg_set_cam_shutter_get_cam_no(msg); - set_cam_shutter->cam_mode = mavlink_msg_set_cam_shutter_get_cam_mode(msg); - set_cam_shutter->trigger_pin = mavlink_msg_set_cam_shutter_get_trigger_pin(msg); - set_cam_shutter->interval = mavlink_msg_set_cam_shutter_get_interval(msg); - set_cam_shutter->exposure = mavlink_msg_set_cam_shutter_get_exposure(msg); - set_cam_shutter->gain = mavlink_msg_set_cam_shutter_get_gain(msg); + memcpy( set_cam_shutter, msg->payload, sizeof(mavlink_set_cam_shutter_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h index 6e349eafc..c27e22bc2 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VICON_POSITION_ESTIMATE PACKING #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 112 +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_112_LEN 32 typedef struct __mavlink_vicon_position_estimate_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_vicon_position_estimate_t } mavlink_vicon_position_estimate_t; - - /** * @brief Pack a vicon_position_estimate message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_vicon_position_estimate_t */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_vicon_position_estimate_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 roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system * @param yaw Yaw angle in rad */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_message_t msg; - mavlink_msg_vicon_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; + msg.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_vicon_position_estimate_t payload; + uint16_t checksum; + mavlink_vicon_position_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch */ static inline uint64_t mavlink_msg_vicon_position_estimate_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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_vicon_position_estimate_get_usec(const mavlin */ static inline float mavlink_msg_vicon_position_estimate_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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_x(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_y(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_z(const mavlink_mess */ static inline float mavlink_msg_vicon_position_estimate_get_roll(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_roll(const mavlink_m */ static inline float mavlink_msg_vicon_position_estimate_get_pitch(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_vicon_position_estimate_get_pitch(const mavlink_ */ static inline float mavlink_msg_vicon_position_estimate_get_yaw(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; + mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_me */ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_message_t* msg, mavlink_vicon_position_estimate_t* vicon_position_estimate) { - vicon_position_estimate->usec = mavlink_msg_vicon_position_estimate_get_usec(msg); - vicon_position_estimate->x = mavlink_msg_vicon_position_estimate_get_x(msg); - vicon_position_estimate->y = mavlink_msg_vicon_position_estimate_get_y(msg); - vicon_position_estimate->z = mavlink_msg_vicon_position_estimate_get_z(msg); - vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); - vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); - vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); + memcpy( vicon_position_estimate, msg->payload, sizeof(mavlink_vicon_position_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h index 30728191d..dbf29c50a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VISION_POSITION_ESTIMATE PACKING #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 111 +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_111_LEN 32 typedef struct __mavlink_vision_position_estimate_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_vision_position_estimate_t } mavlink_vision_position_estimate_t; - - /** * @brief Pack a vision_position_estimate message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_vision_position_estimate_t */ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ */ static inline uint16_t mavlink_msg_vision_position_estimate_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 roll, float pitch, float yaw) { - uint16_t i = 0; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X position - i += put_float_by_index(y, i, msg->payload); // Global Y position - i += put_float_by_index(z, i, msg->payload); // Global Z position - i += put_float_by_index(roll, i, msg->payload); // Roll angle in rad - i += put_float_by_index(pitch, i, msg->payload); // Pitch angle in rad - i += put_float_by_index(yaw, i, msg->payload); // Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste * @param yaw Yaw angle in rad */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_message_t msg; - mavlink_msg_vision_position_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z, roll, pitch, yaw); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; + msg.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_vision_position_estimate_t payload; + uint16_t checksum; + mavlink_vision_position_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c */ static inline uint64_t mavlink_msg_vision_position_estimate_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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_vision_position_estimate_get_usec(const mavli */ static inline float mavlink_msg_vision_position_estimate_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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_vision_position_estimate_get_x(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_vision_position_estimate_get_y(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_vision_position_estimate_get_z(const mavlink_mes */ static inline float mavlink_msg_vision_position_estimate_get_roll(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->roll); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_vision_position_estimate_get_roll(const mavlink_ */ static inline float mavlink_msg_vision_position_estimate_get_pitch(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->pitch); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_vision_position_estimate_get_pitch(const mavlink */ static inline float mavlink_msg_vision_position_estimate_get_yaw(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; + mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; + return (float)(p->yaw); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_m */ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_message_t* msg, mavlink_vision_position_estimate_t* vision_position_estimate) { - vision_position_estimate->usec = mavlink_msg_vision_position_estimate_get_usec(msg); - vision_position_estimate->x = mavlink_msg_vision_position_estimate_get_x(msg); - vision_position_estimate->y = mavlink_msg_vision_position_estimate_get_y(msg); - vision_position_estimate->z = mavlink_msg_vision_position_estimate_get_z(msg); - vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); - vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); - vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); + memcpy( vision_position_estimate, msg->payload, sizeof(mavlink_vision_position_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h index 66224c28e..569f7123b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -1,6 +1,8 @@ // MESSAGE VISION_SPEED_ESTIMATE PACKING #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113 +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_113_LEN 20 typedef struct __mavlink_vision_speed_estimate_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_vision_speed_estimate_t } mavlink_vision_speed_estimate_t; - - /** * @brief Pack a vision_speed_estimate message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_vision_speed_estimate_t */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z) { - uint16_t i = 0; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, */ static inline uint16_t mavlink_msg_vision_speed_estimate_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) { - uint16_t i = 0; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds) - i += put_float_by_index(x, i, msg->payload); // Global X speed - i += put_float_by_index(y, i, msg->payload); // Global Y speed - i += put_float_by_index(z, i, msg->payload); // Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global 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, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i * @param z Global Z speed */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) { mavlink_message_t msg; - mavlink_msg_vision_speed_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; + msg.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +{ + mavlink_header_t hdr; + mavlink_vision_speed_estimate_t payload; + uint16_t checksum; + mavlink_vision_speed_estimate_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,16 +155,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan */ static inline uint64_t mavlink_msg_vision_speed_estimate_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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -123,12 +166,8 @@ static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_ */ static inline float mavlink_msg_vision_speed_estimate_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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->x); } /** @@ -138,12 +177,8 @@ static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_messag */ static inline float mavlink_msg_vision_speed_estimate_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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->y); } /** @@ -153,12 +188,8 @@ static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_messag */ static inline float mavlink_msg_vision_speed_estimate_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; + mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; + return (float)(p->z); } /** @@ -169,8 +200,5 @@ static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_messag */ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate) { - vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg); - vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); - vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); - vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); + memcpy( vision_speed_estimate, msg->payload, sizeof(mavlink_vision_speed_estimate_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index 25ea0f5f1..a039a8aca 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_COMMAND PACKING #define MAVLINK_MSG_ID_WATCHDOG_COMMAND 153 +#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 +#define MAVLINK_MSG_153_LEN 6 typedef struct __mavlink_watchdog_command_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_watchdog_command_t } mavlink_watchdog_command_t; - - /** * @brief Pack a watchdog_command message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_watchdog_command_t */ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - uint16_t i = 0; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { - uint16_t i = 0; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - i += put_uint8_t_by_index(target_system_id, i, msg->payload); // Target system ID - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(command_id, i, msg->payload); // Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui * @param command_id Command ID */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { mavlink_message_t msg; - mavlink_msg_watchdog_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system_id, watchdog_id, process_id, command_id); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg.payload[0]; + + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) +{ + mavlink_header_t hdr; + mavlink_watchdog_command_t payload; + uint16_t checksum; + mavlink_watchdog_command_t *p = &payload; + + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,7 +155,8 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint8_t)(p->target_system_id); } /** @@ -114,10 +166,8 @@ static inline uint8_t mavlink_msg_watchdog_command_get_target_system_id(const ma */ static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -127,10 +177,8 @@ static inline uint16_t mavlink_msg_watchdog_command_get_watchdog_id(const mavlin */ static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint16_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -140,7 +188,8 @@ static inline uint16_t mavlink_msg_watchdog_command_get_process_id(const mavlink */ static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; + return (uint8_t)(p->command_id); } /** @@ -151,8 +200,5 @@ static inline uint8_t mavlink_msg_watchdog_command_get_command_id(const mavlink_ */ static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* msg, mavlink_watchdog_command_t* watchdog_command) { - watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); - watchdog_command->watchdog_id = mavlink_msg_watchdog_command_get_watchdog_id(msg); - watchdog_command->process_id = mavlink_msg_watchdog_command_get_process_id(msg); - watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); + memcpy( watchdog_command, msg->payload, sizeof(mavlink_watchdog_command_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index db693bc55..4d5c2687e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_HEARTBEAT PACKING #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 150 +#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 +#define MAVLINK_MSG_150_LEN 4 typedef struct __mavlink_watchdog_heartbeat_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_watchdog_heartbeat_t } mavlink_watchdog_heartbeat_t; - - /** * @brief Pack a watchdog_heartbeat message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_watchdog_heartbeat_t */ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_count) { - uint16_t i = 0; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_count) { - uint16_t i = 0; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_count, i, msg->payload); // Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, * @param process_count Number of processes */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) { mavlink_message_t msg; - mavlink_msg_watchdog_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_count); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg.payload[0]; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) +{ + mavlink_header_t hdr; + mavlink_watchdog_heartbeat_t payload; + uint16_t checksum; + mavlink_watchdog_heartbeat_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,10 +139,8 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u */ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(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; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -105,10 +150,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_watchdog_id(const mavl */ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(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; + mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; + return (uint16_t)(p->process_count); } /** @@ -119,6 +162,5 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_get_process_count(const ma */ static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t* msg, mavlink_watchdog_heartbeat_t* watchdog_heartbeat) { - watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); - watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); + memcpy( watchdog_heartbeat, msg->payload, sizeof(mavlink_watchdog_heartbeat_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h index a312a1467..44a5cf243 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -1,21 +1,21 @@ // MESSAGE WATCHDOG_PROCESS_INFO PACKING #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 151 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 +#define MAVLINK_MSG_151_LEN 255 typedef struct __mavlink_watchdog_process_info_t { uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID - int8_t name[100]; ///< Process name - int8_t arguments[147]; ///< Process arguments + char name[100]; ///< Process name + char arguments[147]; ///< Process arguments int32_t timeout; ///< Timeout (seconds) } mavlink_watchdog_process_info_t; - #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_ARGUMENTS_LEN 147 - /** * @brief Pack a watchdog_process_info message * @param system_id ID of this system @@ -29,18 +29,18 @@ typedef struct __mavlink_watchdog_process_info_t * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - uint16_t i = 0; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } /** @@ -56,18 +56,18 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { - uint16_t i = 0; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_array_by_index(name, 100, i, msg->payload); // Process name - i += put_array_by_index(arguments, 147, i, msg->payload); // Process arguments - i += put_int32_t_by_index(timeout, i, msg->payload); // Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } /** @@ -94,12 +94,65 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param timeout Timeout (seconds) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const int8_t* name, const int8_t* arguments, int32_t timeout) +static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { mavlink_message_t msg; - mavlink_msg_watchdog_process_info_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, name, arguments, timeout); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg.payload[0]; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) +{ + mavlink_header_t hdr; + mavlink_watchdog_process_info_t payload; + uint16_t checksum; + mavlink_watchdog_process_info_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -112,10 +165,8 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan */ static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(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; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -125,10 +176,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_watchdog_id(const m */ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(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; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -136,11 +185,12 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const ma * * @return Process name */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char* name) { + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t), 100); - return 100; + memcpy(name, p->name, sizeof(p->name)); + return sizeof(p->name); } /** @@ -148,11 +198,12 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_ * * @return Process arguments */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, int8_t* r_data) +static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char* arguments) { + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100, 147); - return 147; + memcpy(arguments, p->arguments, sizeof(p->arguments)); + return sizeof(p->arguments); } /** @@ -162,12 +213,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mav */ static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+100+147)[3]; - return (int32_t)r.i; + mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; + return (int32_t)(p->timeout); } /** @@ -178,9 +225,5 @@ static inline int32_t mavlink_msg_watchdog_process_info_get_timeout(const mavlin */ static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_message_t* msg, mavlink_watchdog_process_info_t* watchdog_process_info) { - watchdog_process_info->watchdog_id = mavlink_msg_watchdog_process_info_get_watchdog_id(msg); - watchdog_process_info->process_id = mavlink_msg_watchdog_process_info_get_process_id(msg); - mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name); - mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments); - watchdog_process_info->timeout = mavlink_msg_watchdog_process_info_get_timeout(msg); + memcpy( watchdog_process_info, msg->payload, sizeof(mavlink_watchdog_process_info_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h index 9afa6ca14..01c80eb31 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -1,6 +1,8 @@ // MESSAGE WATCHDOG_PROCESS_STATUS PACKING #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 152 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 +#define MAVLINK_MSG_152_LEN 12 typedef struct __mavlink_watchdog_process_status_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_watchdog_process_status_t } mavlink_watchdog_process_status_t; - - /** * @brief Pack a watchdog_process_status message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_watchdog_process_status_t */ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - uint16_t i = 0; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i */ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { - uint16_t i = 0; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - i += put_uint16_t_by_index(watchdog_id, i, msg->payload); // Watchdog ID - i += put_uint16_t_by_index(process_id, i, msg->payload); // Process ID - i += put_uint8_t_by_index(state, i, msg->payload); // Is running / finished / suspended / crashed - i += put_uint8_t_by_index(muted, i, msg->payload); // Is muted - i += put_int32_t_by_index(pid, i, msg->payload); // PID - i += put_uint16_t_by_index(crashes, i, msg->payload); // Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system * @param crashes Number of crashes */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { mavlink_message_t msg; - mavlink_msg_watchdog_process_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, watchdog_id, process_id, state, muted, pid, crashes); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg.payload[0]; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) +{ + mavlink_header_t hdr; + mavlink_watchdog_process_status_t payload; + uint16_t checksum; + mavlink_watchdog_process_status_t *p = &payload; + + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,10 +171,8 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch */ static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(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; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->watchdog_id); } /** @@ -129,10 +182,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_watchdog_id(const */ static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(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; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->process_id); } /** @@ -142,7 +193,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_process_id(const */ static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t))[0]; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint8_t)(p->state); } /** @@ -152,7 +204,8 @@ static inline uint8_t mavlink_msg_watchdog_process_status_get_state(const mavlin */ static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t))[0]; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint8_t)(p->muted); } /** @@ -162,12 +215,8 @@ static inline uint8_t mavlink_msg_watchdog_process_status_get_muted(const mavlin */ static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_message_t* msg) { - generic_32bit r; - r.b[3] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; - r.b[2] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[1]; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[2]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t))[3]; - return (int32_t)r.i; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (int32_t)(p->pid); } /** @@ -177,10 +226,8 @@ static inline int32_t mavlink_msg_watchdog_process_status_get_pid(const mavlink_ */ static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[0]; - r.b[0] = (msg->payload+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int32_t))[1]; - return (uint16_t)r.s; + mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; + return (uint16_t)(p->crashes); } /** @@ -191,10 +238,5 @@ static inline uint16_t mavlink_msg_watchdog_process_status_get_crashes(const mav */ static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_message_t* msg, mavlink_watchdog_process_status_t* watchdog_process_status) { - watchdog_process_status->watchdog_id = mavlink_msg_watchdog_process_status_get_watchdog_id(msg); - watchdog_process_status->process_id = mavlink_msg_watchdog_process_status_get_process_id(msg); - watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg); - watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); - watchdog_process_status->pid = mavlink_msg_watchdog_process_status_get_pid(msg); - watchdog_process_status->crashes = mavlink_msg_watchdog_process_status_get_crashes(msg); + memcpy( watchdog_process_status, msg->payload, sizeof(mavlink_watchdog_process_status_t)); } diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 30474d09c..618dbe87c 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -30,7 +30,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Content Types for data transmission handshake */ +/** @brief Content Types for data transmission handshake */ enum DATA_TYPES { DATA_TYPE_JPEG_IMAGE=1, @@ -66,13 +66,6 @@ enum DATA_TYPES #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" #include "./mavlink_msg_brief_feature.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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 diff --git a/thirdParty/mavlink/include/protocol.h b/thirdParty/mavlink/include/protocol.h index ed1f9546b..e9cca2e22 100644 --- a/thirdParty/mavlink/include/protocol.h +++ b/thirdParty/mavlink/include/protocol.h @@ -2,10 +2,13 @@ #define _MAVLINK_PROTOCOL_H_ #include "string.h" -#include "checksum.h" - #include "mavlink_types.h" +#include "checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t mavlink_msg_lengths[256]; +#endif /** * @brief Initialize the communication stack @@ -34,7 +37,12 @@ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { - static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +#else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +#endif + return &m_mavlink_status[chan]; } @@ -68,8 +76,9 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t 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); + 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); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -98,7 +107,8 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin // 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); +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte @@ -111,11 +121,13 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin 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; +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< 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; +// return 0; } /** @@ -197,8 +209,13 @@ static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) */ 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 mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; - +#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +#elif defined(NB_MAVLINK_COMM) + static mavlink_message_t m_mavlink_message[NB_MAVLINK_COMM]; +#else + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +#endif // Initializes only once, values keep unchanged after first initialization mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); @@ -258,6 +275,11 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag case MAVLINK_PARSE_STATE_GOT_COMPID: rxmsg->msgid = c; mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != mavlink_msg_lengths[c] ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif if (rxmsg->len == 0) { status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; @@ -313,7 +335,9 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag // Successfully got message status->msg_received = 1; status->parse_state = MAVLINK_PARSE_STATE_IDLE; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; } break; } @@ -338,10 +362,86 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag 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; + + // For future use + +// if (status->msg_received == 1) +// { +// if ( r_message != NULL ) +// { +// return r_message; +// } +// else +// { +// return rxmsg; +// } +// } +// else +// { +// return NULL; +// } return status->msg_received; } +#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_msg(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); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#define FILE_FINISHED + +#ifndef FILE_FINISHED /** * 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 @@ -383,13 +483,23 @@ static inline uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messag /* 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) { - 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]; + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; + #else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; + #endif // Set a packet start candidate index if sign is start sign if (c == MAVLINK_STX) @@ -399,8 +509,13 @@ static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_me // Parse normally, if a CRC mismatch occurs retry with the next packet index } -// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS]; -// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS]; +//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +//#else +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +//#endif //// Initializes only once, values keep unchanged after first initialization // mavlink_parse_state_initialize(&m_mavlink_status[chan]); // @@ -552,7 +667,6 @@ 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; /** @@ -839,144 +953,7 @@ static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t p 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); -} -#endif +*/ +#endif /* FILE_FINISHED */ #endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index bf0e146b4..1379932f2 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "slugs.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 2b212ae1b..19503c1df 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -1,6 +1,8 @@ // MESSAGE AIR_DATA PACKING #define MAVLINK_MSG_ID_AIR_DATA 171 +#define MAVLINK_MSG_ID_AIR_DATA_LEN 10 +#define MAVLINK_MSG_171_LEN 10 typedef struct __mavlink_air_data_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_air_data_t } mavlink_air_data_t; - - /** * @brief Pack a air_data message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_air_data_t */ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) { - uint16_t i = 0; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIR_DATA_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float dynamicPressure, float staticPressure, uint16_t temperature) { - uint16_t i = 0; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - i += put_float_by_index(dynamicPressure, i, msg->payload); // Dynamic pressure (Pa) - i += put_float_by_index(staticPressure, i, msg->payload); // Static pressure (Pa) - i += put_uint16_t_by_index(temperature, i, msg->payload); // Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIR_DATA_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co * @param temperature Board temperature */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) { mavlink_message_t msg; - mavlink_msg_air_data_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, dynamicPressure, staticPressure, temperature); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg.payload[0]; + + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_AIR_DATA_LEN; + msg.msgid = MAVLINK_MSG_ID_AIR_DATA; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) +{ + mavlink_header_t hdr; + mavlink_air_data_t payload; + uint16_t checksum; + mavlink_air_data_t *p = &payload; + + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_AIR_DATA_LEN; + hdr.msgid = MAVLINK_MSG_ID_AIR_DATA; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,12 +147,8 @@ static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynam */ static inline float mavlink_msg_air_data_get_dynamicPressure(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; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (float)(p->dynamicPressure); } /** @@ -113,12 +158,8 @@ static inline float mavlink_msg_air_data_get_dynamicPressure(const mavlink_messa */ static inline float mavlink_msg_air_data_get_staticPressure(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; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (float)(p->staticPressure); } /** @@ -128,10 +169,8 @@ static inline float mavlink_msg_air_data_get_staticPressure(const mavlink_messag */ static inline uint16_t mavlink_msg_air_data_get_temperature(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 (uint16_t)r.s; + mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; + return (uint16_t)(p->temperature); } /** @@ -142,7 +181,5 @@ static inline uint16_t mavlink_msg_air_data_get_temperature(const mavlink_messag */ static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mavlink_air_data_t* air_data) { - air_data->dynamicPressure = mavlink_msg_air_data_get_dynamicPressure(msg); - air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg); - air_data->temperature = mavlink_msg_air_data_get_temperature(msg); + memcpy( air_data, msg->payload, sizeof(mavlink_air_data_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index 265aa3fce..d3191546a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -1,6 +1,8 @@ // MESSAGE CPU_LOAD PACKING #define MAVLINK_MSG_ID_CPU_LOAD 170 +#define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 +#define MAVLINK_MSG_170_LEN 4 typedef struct __mavlink_cpu_load_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_cpu_load_t } mavlink_cpu_load_t; - - /** * @brief Pack a cpu_load message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_cpu_load_t */ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - uint16_t i = 0; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CPU_LOAD_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - uint16_t i = 0; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - i += put_uint8_t_by_index(sensLoad, i, msg->payload); // Sensor DSC Load - i += put_uint8_t_by_index(ctrlLoad, i, msg->payload); // Control DSC Load - i += put_uint16_t_by_index(batVolt, i, msg->payload); // Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CPU_LOAD_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co * @param batVolt Battery Voltage in millivolts */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { mavlink_message_t msg; - mavlink_msg_cpu_load_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, sensLoad, ctrlLoad, batVolt); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg.payload[0]; + + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; + msg.msgid = MAVLINK_MSG_ID_CPU_LOAD; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) +{ + mavlink_header_t hdr; + mavlink_cpu_load_t payload; + uint16_t checksum; + mavlink_cpu_load_t *p = &payload; + + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; + hdr.msgid = MAVLINK_MSG_ID_CPU_LOAD; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sen */ static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint8_t)(p->sensLoad); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_cpu_load_get_sensLoad(const mavlink_message_t* */ static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint8_t)(p->ctrlLoad); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_cpu_load_get_ctrlLoad(const mavlink_message_t* */ static inline uint16_t mavlink_msg_cpu_load_get_batVolt(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; + mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; + return (uint16_t)(p->batVolt); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_cpu_load_get_batVolt(const mavlink_message_t* */ static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mavlink_cpu_load_t* cpu_load) { - cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); - cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); - cpu_load->batVolt = mavlink_msg_cpu_load_get_batVolt(msg); + memcpy( cpu_load, msg->payload, sizeof(mavlink_cpu_load_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h index 9221dc4f0..f983a544c 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -1,6 +1,8 @@ // MESSAGE CTRL_SRFC_PT PACKING #define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 +#define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 +#define MAVLINK_MSG_181_LEN 3 typedef struct __mavlink_ctrl_srfc_pt_t { @@ -9,8 +11,6 @@ typedef struct __mavlink_ctrl_srfc_pt_t } mavlink_ctrl_srfc_pt_t; - - /** * @brief Pack a ctrl_srfc_pt message * @param system_id ID of this system @@ -23,13 +23,13 @@ typedef struct __mavlink_ctrl_srfc_pt_t */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) { - uint16_t i = 0; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } /** @@ -44,13 +44,13 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint16_t bitfieldPt) { - uint16_t i = 0; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_uint16_t_by_index(bitfieldPt, i, msg->payload); // Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } /** @@ -74,12 +74,59 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ * @param bitfieldPt Bitfield containing the PT configuration */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) { mavlink_message_t msg; - mavlink_msg_ctrl_srfc_pt_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, bitfieldPt); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; + msg.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) +{ + mavlink_header_t hdr; + mavlink_ctrl_srfc_pt_t payload; + uint16_t checksum; + mavlink_ctrl_srfc_pt_t *p = &payload; + + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; + hdr.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -92,7 +139,8 @@ static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -102,10 +150,8 @@ static inline uint8_t mavlink_msg_ctrl_srfc_pt_get_target(const mavlink_message_ */ static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(uint8_t))[0]; - r.b[0] = (msg->payload+sizeof(uint8_t))[1]; - return (uint16_t)r.s; + mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; + return (uint16_t)(p->bitfieldPt); } /** @@ -116,6 +162,5 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(const mavlink_mes */ static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, mavlink_ctrl_srfc_pt_t* ctrl_srfc_pt) { - ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); - ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); + memcpy( ctrl_srfc_pt, msg->payload, sizeof(mavlink_ctrl_srfc_pt_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index cbcc0daba..018de9224 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -1,6 +1,8 @@ // MESSAGE DATA_LOG PACKING #define MAVLINK_MSG_ID_DATA_LOG 177 +#define MAVLINK_MSG_ID_DATA_LOG_LEN 24 +#define MAVLINK_MSG_177_LEN 24 typedef struct __mavlink_data_log_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_data_log_t } mavlink_data_log_t; - - /** * @brief Pack a data_log message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_data_log_t */ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - uint16_t i = 0; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_LOG_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { - uint16_t i = 0; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - i += put_float_by_index(fl_1, i, msg->payload); // Log value 1 - i += put_float_by_index(fl_2, i, msg->payload); // Log value 2 - i += put_float_by_index(fl_3, i, msg->payload); // Log value 3 - i += put_float_by_index(fl_4, i, msg->payload); // Log value 4 - i += put_float_by_index(fl_5, i, msg->payload); // Log value 5 - i += put_float_by_index(fl_6, i, msg->payload); // Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_LOG_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co * @param fl_6 Log value 6 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { mavlink_message_t msg; - mavlink_msg_data_log_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, fl_1, fl_2, fl_3, fl_4, fl_5, fl_6); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg.payload[0]; + + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DATA_LOG_LEN; + msg.msgid = MAVLINK_MSG_ID_DATA_LOG; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) +{ + mavlink_header_t hdr; + mavlink_data_log_t payload; + uint16_t checksum; + mavlink_data_log_t *p = &payload; + + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DATA_LOG_LEN; + hdr.msgid = MAVLINK_MSG_ID_DATA_LOG; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, */ static inline float mavlink_msg_data_log_get_fl_1(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; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_1); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_data_log_get_fl_1(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_2(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; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_2); } /** @@ -146,12 +193,8 @@ static inline float mavlink_msg_data_log_get_fl_2(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_3(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; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_3); } /** @@ -161,12 +204,8 @@ static inline float mavlink_msg_data_log_get_fl_3(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_4(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; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_4); } /** @@ -176,12 +215,8 @@ static inline float mavlink_msg_data_log_get_fl_4(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_5(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; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_5); } /** @@ -191,12 +226,8 @@ static inline float mavlink_msg_data_log_get_fl_5(const mavlink_message_t* msg) */ static inline float mavlink_msg_data_log_get_fl_6(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; + mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; + return (float)(p->fl_6); } /** @@ -207,10 +238,5 @@ static inline float mavlink_msg_data_log_get_fl_6(const mavlink_message_t* msg) */ static inline void mavlink_msg_data_log_decode(const mavlink_message_t* msg, mavlink_data_log_t* data_log) { - data_log->fl_1 = mavlink_msg_data_log_get_fl_1(msg); - data_log->fl_2 = mavlink_msg_data_log_get_fl_2(msg); - data_log->fl_3 = mavlink_msg_data_log_get_fl_3(msg); - data_log->fl_4 = mavlink_msg_data_log_get_fl_4(msg); - data_log->fl_5 = mavlink_msg_data_log_get_fl_5(msg); - data_log->fl_6 = mavlink_msg_data_log_get_fl_6(msg); + memcpy( data_log, msg->payload, sizeof(mavlink_data_log_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index f3798f59b..4bfebe5fb 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -1,6 +1,8 @@ // MESSAGE DIAGNOSTIC PACKING #define MAVLINK_MSG_ID_DIAGNOSTIC 173 +#define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 +#define MAVLINK_MSG_173_LEN 18 typedef struct __mavlink_diagnostic_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_diagnostic_t } mavlink_diagnostic_t; - - /** * @brief Pack a diagnostic message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_diagnostic_t */ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - uint16_t i = 0; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { - uint16_t i = 0; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - i += put_float_by_index(diagFl1, i, msg->payload); // Diagnostic float 1 - i += put_float_by_index(diagFl2, i, msg->payload); // Diagnostic float 2 - i += put_float_by_index(diagFl3, i, msg->payload); // Diagnostic float 3 - i += put_int16_t_by_index(diagSh1, i, msg->payload); // Diagnostic short 1 - i += put_int16_t_by_index(diagSh2, i, msg->payload); // Diagnostic short 2 - i += put_int16_t_by_index(diagSh3, i, msg->payload); // Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t * @param diagSh3 Diagnostic short 3 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { mavlink_message_t msg; - mavlink_msg_diagnostic_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, diagFl1, diagFl2, diagFl3, diagSh1, diagSh2, diagSh3); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg.payload[0]; + + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; + msg.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) +{ + mavlink_header_t hdr; + mavlink_diagnostic_t payload; + uint16_t checksum; + mavlink_diagnostic_t *p = &payload; + + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; + hdr.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float dia */ static inline float mavlink_msg_diagnostic_get_diagFl1(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; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl1); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl1(const mavlink_message_t* */ static inline float mavlink_msg_diagnostic_get_diagFl2(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; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl2); } /** @@ -146,12 +193,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl2(const mavlink_message_t* */ static inline float mavlink_msg_diagnostic_get_diagFl3(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; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (float)(p->diagFl3); } /** @@ -161,10 +204,8 @@ static inline float mavlink_msg_diagnostic_get_diagFl3(const mavlink_message_t* */ static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh1); } /** @@ -174,10 +215,8 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh1(const mavlink_message_t */ static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh2); } /** @@ -187,10 +226,8 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh2(const mavlink_message_t */ static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t* msg) { - generic_16bit r; - r.b[1] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[0]; - r.b[0] = (msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int16_t)+sizeof(int16_t))[1]; - return (int16_t)r.s; + mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; + return (int16_t)(p->diagSh3); } /** @@ -201,10 +238,5 @@ static inline int16_t mavlink_msg_diagnostic_get_diagSh3(const mavlink_message_t */ static inline void mavlink_msg_diagnostic_decode(const mavlink_message_t* msg, mavlink_diagnostic_t* diagnostic) { - diagnostic->diagFl1 = mavlink_msg_diagnostic_get_diagFl1(msg); - diagnostic->diagFl2 = mavlink_msg_diagnostic_get_diagFl2(msg); - diagnostic->diagFl3 = mavlink_msg_diagnostic_get_diagFl3(msg); - diagnostic->diagSh1 = mavlink_msg_diagnostic_get_diagSh1(msg); - diagnostic->diagSh2 = mavlink_msg_diagnostic_get_diagSh2(msg); - diagnostic->diagSh3 = mavlink_msg_diagnostic_get_diagSh3(msg); + memcpy( diagnostic, msg->payload, sizeof(mavlink_diagnostic_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h index 199cbb9ec..ca3b1a934 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -1,6 +1,8 @@ // MESSAGE GPS_DATE_TIME PACKING #define MAVLINK_MSG_ID_GPS_DATE_TIME 179 +#define MAVLINK_MSG_ID_GPS_DATE_TIME_LEN 7 +#define MAVLINK_MSG_179_LEN 7 typedef struct __mavlink_gps_date_time_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_gps_date_time_t } mavlink_gps_date_time_t; - - /** * @brief Pack a gps_date_time message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_gps_date_time_t */ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - uint16_t i = 0; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { - uint16_t i = 0; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - i += put_uint8_t_by_index(year, i, msg->payload); // Year reported by Gps - i += put_uint8_t_by_index(month, i, msg->payload); // Month reported by Gps - i += put_uint8_t_by_index(day, i, msg->payload); // Day reported by Gps - i += put_uint8_t_by_index(hour, i, msg->payload); // Hour reported by Gps - i += put_uint8_t_by_index(min, i, msg->payload); // Min reported by Gps - i += put_uint8_t_by_index(sec, i, msg->payload); // Sec reported by Gps - i += put_uint8_t_by_index(visSat, i, msg->payload); // Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 * @param visSat Visible sattelites reported by Gps */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { mavlink_message_t msg; - mavlink_msg_gps_date_time_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, year, month, day, hour, min, sec, visSat); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg.payload[0]; + + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; + msg.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) +{ + mavlink_header_t hdr; + mavlink_gps_date_time_t payload; + uint16_t checksum; + mavlink_gps_date_time_t *p = &payload; + + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; + hdr.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,7 +179,8 @@ static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_ */ static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->year); } /** @@ -132,7 +190,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_year(const mavlink_message_t */ static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->month); } /** @@ -142,7 +201,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_month(const mavlink_message_ */ static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->day); } /** @@ -152,7 +212,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_day(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->hour); } /** @@ -162,7 +223,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_hour(const mavlink_message_t */ static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->min); } /** @@ -172,7 +234,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_min(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_sec(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]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->sec); } /** @@ -182,7 +245,8 @@ static inline uint8_t mavlink_msg_gps_date_time_get_sec(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_date_time_get_visSat(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]; + mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; + return (uint8_t)(p->visSat); } /** @@ -193,11 +257,5 @@ static inline uint8_t mavlink_msg_gps_date_time_get_visSat(const mavlink_message */ static inline void mavlink_msg_gps_date_time_decode(const mavlink_message_t* msg, mavlink_gps_date_time_t* gps_date_time) { - gps_date_time->year = mavlink_msg_gps_date_time_get_year(msg); - gps_date_time->month = mavlink_msg_gps_date_time_get_month(msg); - gps_date_time->day = mavlink_msg_gps_date_time_get_day(msg); - gps_date_time->hour = mavlink_msg_gps_date_time_get_hour(msg); - gps_date_time->min = mavlink_msg_gps_date_time_get_min(msg); - gps_date_time->sec = mavlink_msg_gps_date_time_get_sec(msg); - gps_date_time->visSat = mavlink_msg_gps_date_time_get_visSat(msg); + memcpy( gps_date_time, msg->payload, sizeof(mavlink_gps_date_time_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h index a0d78b1ce..c15461332 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -1,6 +1,8 @@ // MESSAGE MID_LVL_CMDS PACKING #define MAVLINK_MSG_ID_MID_LVL_CMDS 180 +#define MAVLINK_MSG_ID_MID_LVL_CMDS_LEN 13 +#define MAVLINK_MSG_180_LEN 13 typedef struct __mavlink_mid_lvl_cmds_t { @@ -11,8 +13,6 @@ typedef struct __mavlink_mid_lvl_cmds_t } mavlink_mid_lvl_cmds_t; - - /** * @brief Pack a mid_lvl_cmds message * @param system_id ID of this system @@ -27,15 +27,15 @@ typedef struct __mavlink_mid_lvl_cmds_t */ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) { - uint16_t i = 0; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } /** @@ -52,15 +52,15 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, float hCommand, float uCommand, float rCommand) { - uint16_t i = 0; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - i += put_uint8_t_by_index(target, i, msg->payload); // The system setting the commands - i += put_float_by_index(hCommand, i, msg->payload); // Commanded Airspeed - i += put_float_by_index(uCommand, i, msg->payload); // Log value 2 - i += put_float_by_index(rCommand, i, msg->payload); // Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } /** @@ -86,12 +86,63 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ * @param rCommand Log value 3 */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) { mavlink_message_t msg; - mavlink_msg_mid_lvl_cmds_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, hCommand, uCommand, rCommand); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; + msg.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) +{ + mavlink_header_t hdr; + mavlink_mid_lvl_cmds_t payload; + uint16_t checksum; + mavlink_mid_lvl_cmds_t *p = &payload; + + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; + hdr.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -104,7 +155,8 @@ static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -114,12 +166,8 @@ static inline uint8_t mavlink_msg_mid_lvl_cmds_get_target(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(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; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->hCommand); } /** @@ -129,12 +177,8 @@ static inline float mavlink_msg_mid_lvl_cmds_get_hCommand(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(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; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->uCommand); } /** @@ -144,12 +188,8 @@ static inline float mavlink_msg_mid_lvl_cmds_get_uCommand(const mavlink_message_ */ static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(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; + mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; + return (float)(p->rCommand); } /** @@ -160,8 +200,5 @@ static inline float mavlink_msg_mid_lvl_cmds_get_rCommand(const mavlink_message_ */ static inline void mavlink_msg_mid_lvl_cmds_decode(const mavlink_message_t* msg, mavlink_mid_lvl_cmds_t* mid_lvl_cmds) { - mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); - mid_lvl_cmds->hCommand = mavlink_msg_mid_lvl_cmds_get_hCommand(msg); - mid_lvl_cmds->uCommand = mavlink_msg_mid_lvl_cmds_get_uCommand(msg); - mid_lvl_cmds->rCommand = mavlink_msg_mid_lvl_cmds_get_rCommand(msg); + memcpy( mid_lvl_cmds, msg->payload, sizeof(mavlink_mid_lvl_cmds_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 8b40bdd67..944ef8d41 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -1,6 +1,8 @@ // MESSAGE SENSOR_BIAS PACKING #define MAVLINK_MSG_ID_SENSOR_BIAS 172 +#define MAVLINK_MSG_ID_SENSOR_BIAS_LEN 24 +#define MAVLINK_MSG_172_LEN 24 typedef struct __mavlink_sensor_bias_t { @@ -13,8 +15,6 @@ typedef struct __mavlink_sensor_bias_t } mavlink_sensor_bias_t; - - /** * @brief Pack a sensor_bias message * @param system_id ID of this system @@ -31,17 +31,17 @@ typedef struct __mavlink_sensor_bias_t */ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - uint16_t i = 0; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } /** @@ -60,17 +60,17 @@ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { - uint16_t i = 0; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - i += put_float_by_index(axBias, i, msg->payload); // Accelerometer X bias (m/s) - i += put_float_by_index(ayBias, i, msg->payload); // Accelerometer Y bias (m/s) - i += put_float_by_index(azBias, i, msg->payload); // Accelerometer Z bias (m/s) - i += put_float_by_index(gxBias, i, msg->payload); // Gyro X bias (rad/s) - i += put_float_by_index(gyBias, i, msg->payload); // Gyro Y bias (rad/s) - i += put_float_by_index(gzBias, i, msg->payload); // Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (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, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } /** @@ -98,12 +98,67 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t * @param gzBias Gyro Z bias (rad/s) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { mavlink_message_t msg; - mavlink_msg_sensor_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, axBias, ayBias, azBias, gxBias, gyBias, gzBias); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg.payload[0]; + + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; + msg.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) +{ + mavlink_header_t hdr; + mavlink_sensor_bias_t payload; + uint16_t checksum; + mavlink_sensor_bias_t *p = &payload; + + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; + hdr.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -116,12 +171,8 @@ static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float ax */ static inline float mavlink_msg_sensor_bias_get_axBias(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; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->axBias); } /** @@ -131,12 +182,8 @@ static inline float mavlink_msg_sensor_bias_get_axBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_ayBias(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; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->ayBias); } /** @@ -146,12 +193,8 @@ static inline float mavlink_msg_sensor_bias_get_ayBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_azBias(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; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->azBias); } /** @@ -161,12 +204,8 @@ static inline float mavlink_msg_sensor_bias_get_azBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gxBias(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; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gxBias); } /** @@ -176,12 +215,8 @@ static inline float mavlink_msg_sensor_bias_get_gxBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gyBias(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; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gyBias); } /** @@ -191,12 +226,8 @@ static inline float mavlink_msg_sensor_bias_get_gyBias(const mavlink_message_t* */ static inline float mavlink_msg_sensor_bias_get_gzBias(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; + mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; + return (float)(p->gzBias); } /** @@ -207,10 +238,5 @@ static inline float mavlink_msg_sensor_bias_get_gzBias(const mavlink_message_t* */ static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, mavlink_sensor_bias_t* sensor_bias) { - sensor_bias->axBias = mavlink_msg_sensor_bias_get_axBias(msg); - sensor_bias->ayBias = mavlink_msg_sensor_bias_get_ayBias(msg); - sensor_bias->azBias = mavlink_msg_sensor_bias_get_azBias(msg); - sensor_bias->gxBias = mavlink_msg_sensor_bias_get_gxBias(msg); - sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg); - sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg); + memcpy( sensor_bias, msg->payload, sizeof(mavlink_sensor_bias_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index 066416656..d86066689 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -1,6 +1,8 @@ // MESSAGE SLUGS_ACTION PACKING #define MAVLINK_MSG_ID_SLUGS_ACTION 183 +#define MAVLINK_MSG_ID_SLUGS_ACTION_LEN 4 +#define MAVLINK_MSG_183_LEN 4 typedef struct __mavlink_slugs_action_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_slugs_action_t } mavlink_slugs_action_t; - - /** * @brief Pack a slugs_action message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_slugs_action_t */ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) { - uint16_t i = 0; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target, uint8_t actionId, uint16_t actionVal) { - uint16_t i = 0; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - i += put_uint8_t_by_index(target, i, msg->payload); // The system reporting the action - i += put_uint8_t_by_index(actionId, i, msg->payload); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - i += put_uint16_t_by_index(actionVal, i, msg->payload); // Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ * @param actionVal Value associated with the action */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) { mavlink_message_t msg; - mavlink_msg_slugs_action_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target, actionId, actionVal); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg.payload[0]; + + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; + msg.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) +{ + mavlink_header_t hdr; + mavlink_slugs_action_t payload; + uint16_t checksum; + mavlink_slugs_action_t *p = &payload; + + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; + hdr.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint8_t)(p->target); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_slugs_action_get_target(const mavlink_message_ */ static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint8_t)(p->actionId); } /** @@ -118,10 +169,8 @@ static inline uint8_t mavlink_msg_slugs_action_get_actionId(const mavlink_messag */ static inline uint16_t mavlink_msg_slugs_action_get_actionVal(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; + mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; + return (uint16_t)(p->actionVal); } /** @@ -132,7 +181,5 @@ static inline uint16_t mavlink_msg_slugs_action_get_actionVal(const mavlink_mess */ static inline void mavlink_msg_slugs_action_decode(const mavlink_message_t* msg, mavlink_slugs_action_t* slugs_action) { - slugs_action->target = mavlink_msg_slugs_action_get_target(msg); - slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); - slugs_action->actionVal = mavlink_msg_slugs_action_get_actionVal(msg); + memcpy( slugs_action, msg->payload, sizeof(mavlink_slugs_action_t)); } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index f1a54cb3e..0c8d87f00 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -1,6 +1,8 @@ // MESSAGE SLUGS_NAVIGATION PACKING #define MAVLINK_MSG_ID_SLUGS_NAVIGATION 176 +#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN 30 +#define MAVLINK_MSG_176_LEN 30 typedef struct __mavlink_slugs_navigation_t { @@ -16,8 +18,6 @@ typedef struct __mavlink_slugs_navigation_t } mavlink_slugs_navigation_t; - - /** * @brief Pack a slugs_navigation message * @param system_id ID of this system @@ -37,20 +37,20 @@ typedef struct __mavlink_slugs_navigation_t */ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - uint16_t i = 0; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } /** @@ -72,20 +72,20 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { - uint16_t i = 0; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - i += put_float_by_index(u_m, i, msg->payload); // Measured Airspeed prior to the Nav Filter - i += put_float_by_index(phi_c, i, msg->payload); // Commanded Roll - i += put_float_by_index(theta_c, i, msg->payload); // Commanded Pitch - i += put_float_by_index(psiDot_c, i, msg->payload); // Commanded Turn rate - i += put_float_by_index(ay_body, i, msg->payload); // Y component of the body acceleration - i += put_float_by_index(totalDist, i, msg->payload); // Total Distance to Run on this leg of Navigation - i += put_float_by_index(dist2Go, i, msg->payload); // Remaining distance to Run on this leg of Navigation - i += put_uint8_t_by_index(fromWP, i, msg->payload); // Origin WP - i += put_uint8_t_by_index(toWP, i, msg->payload); // Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } /** @@ -116,12 +116,73 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui * @param toWP Destination WP */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { mavlink_message_t msg; - mavlink_msg_slugs_navigation_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, u_m, phi_c, theta_c, psiDot_c, ay_body, totalDist, dist2Go, fromWP, toWP); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg.payload[0]; + + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; + msg.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) +{ + mavlink_header_t hdr; + mavlink_slugs_navigation_t payload; + uint16_t checksum; + mavlink_slugs_navigation_t *p = &payload; + + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; + hdr.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -134,12 +195,8 @@ static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_slugs_navigation_get_u_m(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->u_m); } /** @@ -149,12 +206,8 @@ static inline float mavlink_msg_slugs_navigation_get_u_m(const mavlink_message_t */ static inline float mavlink_msg_slugs_navigation_get_phi_c(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->phi_c); } /** @@ -164,12 +217,8 @@ static inline float mavlink_msg_slugs_navigation_get_phi_c(const mavlink_message */ static inline float mavlink_msg_slugs_navigation_get_theta_c(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->theta_c); } /** @@ -179,12 +228,8 @@ static inline float mavlink_msg_slugs_navigation_get_theta_c(const mavlink_messa */ static inline float mavlink_msg_slugs_navigation_get_psiDot_c(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->psiDot_c); } /** @@ -194,12 +239,8 @@ static inline float mavlink_msg_slugs_navigation_get_psiDot_c(const mavlink_mess */ static inline float mavlink_msg_slugs_navigation_get_ay_body(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->ay_body); } /** @@ -209,12 +250,8 @@ static inline float mavlink_msg_slugs_navigation_get_ay_body(const mavlink_messa */ static inline float mavlink_msg_slugs_navigation_get_totalDist(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->totalDist); } /** @@ -224,12 +261,8 @@ static inline float mavlink_msg_slugs_navigation_get_totalDist(const mavlink_mes */ static inline float mavlink_msg_slugs_navigation_get_dist2Go(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; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (float)(p->dist2Go); } /** @@ -239,7 +272,8 @@ static inline float mavlink_msg_slugs_navigation_get_dist2Go(const mavlink_messa */ static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0]; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (uint8_t)(p->fromWP); } /** @@ -249,7 +283,8 @@ static inline uint8_t mavlink_msg_slugs_navigation_get_fromWP(const mavlink_mess */ static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0]; + mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; + return (uint8_t)(p->toWP); } /** @@ -260,13 +295,5 @@ static inline uint8_t mavlink_msg_slugs_navigation_get_toWP(const mavlink_messag */ static inline void mavlink_msg_slugs_navigation_decode(const mavlink_message_t* msg, mavlink_slugs_navigation_t* slugs_navigation) { - slugs_navigation->u_m = mavlink_msg_slugs_navigation_get_u_m(msg); - slugs_navigation->phi_c = mavlink_msg_slugs_navigation_get_phi_c(msg); - slugs_navigation->theta_c = mavlink_msg_slugs_navigation_get_theta_c(msg); - slugs_navigation->psiDot_c = mavlink_msg_slugs_navigation_get_psiDot_c(msg); - slugs_navigation->ay_body = mavlink_msg_slugs_navigation_get_ay_body(msg); - slugs_navigation->totalDist = mavlink_msg_slugs_navigation_get_totalDist(msg); - slugs_navigation->dist2Go = mavlink_msg_slugs_navigation_get_dist2Go(msg); - slugs_navigation->fromWP = mavlink_msg_slugs_navigation_get_fromWP(msg); - slugs_navigation->toWP = mavlink_msg_slugs_navigation_get_toWP(msg); + memcpy( slugs_navigation, msg->payload, sizeof(mavlink_slugs_navigation_t)); } diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index b8a6ff330..e742be358 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:11 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -43,13 +43,6 @@ extern "C" { #include "./mavlink_msg_mid_lvl_cmds.h" #include "./mavlink_msg_ctrl_srfc_pt.h" #include "./mavlink_msg_slugs_action.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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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 diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 30b060f63..39a4c44a6 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,11 +1,15 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef MAVLINK_H #define MAVLINK_H +#pragma pack(push,1) #include "ualberta.h" - +#ifdef MAVLINK_CHECK_LENGTH +#include "lengths.h" +#endif +#pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index a011fc1ab..d278887fe 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -1,6 +1,8 @@ // MESSAGE NAV_FILTER_BIAS PACKING #define MAVLINK_MSG_ID_NAV_FILTER_BIAS 220 +#define MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN 32 +#define MAVLINK_MSG_220_LEN 32 typedef struct __mavlink_nav_filter_bias_t { @@ -14,8 +16,6 @@ typedef struct __mavlink_nav_filter_bias_t } mavlink_nav_filter_bias_t; - - /** * @brief Pack a nav_filter_bias message * @param system_id ID of this system @@ -33,18 +33,18 @@ typedef struct __mavlink_nav_filter_bias_t */ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - uint16_t i = 0; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } /** @@ -64,18 +64,18 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { - uint16_t i = 0; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds) - i += put_float_by_index(accel_0, i, msg->payload); // b_f[0] - i += put_float_by_index(accel_1, i, msg->payload); // b_f[1] - i += put_float_by_index(accel_2, i, msg->payload); // b_f[2] - i += put_float_by_index(gyro_0, i, msg->payload); // b_f[0] - i += put_float_by_index(gyro_1, i, msg->payload); // b_f[1] - i += put_float_by_index(gyro_2, i, msg->payload); // b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } /** @@ -104,12 +104,69 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin * @param gyro_2 b_f[2] */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { mavlink_message_t msg; - mavlink_msg_nav_filter_bias_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, accel_0, accel_1, accel_2, gyro_0, gyro_1, gyro_2); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg.payload[0]; + + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; + msg.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) +{ + mavlink_header_t hdr; + mavlink_nav_filter_bias_t payload; + uint16_t checksum; + mavlink_nav_filter_bias_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; + hdr.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -122,16 +179,8 @@ static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint */ static inline uint64_t mavlink_msg_nav_filter_bias_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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (uint64_t)(p->usec); } /** @@ -141,12 +190,8 @@ static inline uint64_t mavlink_msg_nav_filter_bias_get_usec(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_0(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_0); } /** @@ -156,12 +201,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_0(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_1(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_1); } /** @@ -171,12 +212,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_1(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_accel_2(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->accel_2); } /** @@ -186,12 +223,8 @@ static inline float mavlink_msg_nav_filter_bias_get_accel_2(const mavlink_messag */ static inline float mavlink_msg_nav_filter_bias_get_gyro_0(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_0); } /** @@ -201,12 +234,8 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_0(const mavlink_message */ static inline float mavlink_msg_nav_filter_bias_get_gyro_1(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_1); } /** @@ -216,12 +245,8 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_1(const mavlink_message */ static inline float mavlink_msg_nav_filter_bias_get_gyro_2(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; + mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; + return (float)(p->gyro_2); } /** @@ -232,11 +257,5 @@ static inline float mavlink_msg_nav_filter_bias_get_gyro_2(const mavlink_message */ static inline void mavlink_msg_nav_filter_bias_decode(const mavlink_message_t* msg, mavlink_nav_filter_bias_t* nav_filter_bias) { - nav_filter_bias->usec = mavlink_msg_nav_filter_bias_get_usec(msg); - nav_filter_bias->accel_0 = mavlink_msg_nav_filter_bias_get_accel_0(msg); - nav_filter_bias->accel_1 = mavlink_msg_nav_filter_bias_get_accel_1(msg); - nav_filter_bias->accel_2 = mavlink_msg_nav_filter_bias_get_accel_2(msg); - nav_filter_bias->gyro_0 = mavlink_msg_nav_filter_bias_get_gyro_0(msg); - nav_filter_bias->gyro_1 = mavlink_msg_nav_filter_bias_get_gyro_1(msg); - nav_filter_bias->gyro_2 = mavlink_msg_nav_filter_bias_get_gyro_2(msg); + memcpy( nav_filter_bias, msg->payload, sizeof(mavlink_nav_filter_bias_t)); } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 5907aba95..0c5e7972e 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -1,6 +1,8 @@ // MESSAGE RADIO_CALIBRATION PACKING #define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 +#define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42 +#define MAVLINK_MSG_221_LEN 42 typedef struct __mavlink_radio_calibration_t { @@ -12,7 +14,6 @@ typedef struct __mavlink_radio_calibration_t uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) } mavlink_radio_calibration_t; - #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_ELEVATOR_LEN 3 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_RUDDER_LEN 3 @@ -20,7 +21,6 @@ typedef struct __mavlink_radio_calibration_t #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_PITCH_LEN 5 #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_THROTTLE_LEN 5 - /** * @brief Pack a radio_calibration message * @param system_id ID of this system @@ -37,17 +37,17 @@ typedef struct __mavlink_radio_calibration_t */ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - uint16_t i = 0; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } /** @@ -66,17 +66,17 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { - uint16_t i = 0; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right - i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up - i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right - i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode - i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%) - i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } /** @@ -104,12 +104,67 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param throttle Throttle curve setpoints (every 25%) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { mavlink_message_t msg; - mavlink_msg_radio_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, aileron, elevator, rudder, gyro, pitch, throttle); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg.payload[0]; + + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; + msg.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) +{ + mavlink_header_t hdr; + mavlink_radio_calibration_t payload; + uint16_t checksum; + mavlink_radio_calibration_t *p = &payload; + + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; + hdr.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -120,11 +175,12 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co * * @return Aileron setpoints: left, center, right */ -static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* aileron) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(aileron, p->aileron, sizeof(p->aileron)); + return sizeof(p->aileron); } /** @@ -132,11 +188,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_m * * @return Elevator setpoints: nose down, center, nose up */ -static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* elevator) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(elevator, p->elevator, sizeof(p->elevator)); + return sizeof(p->elevator); } /** @@ -144,11 +201,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_ * * @return Rudder setpoints: nose left, center, nose right */ -static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* rudder) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*3); - return sizeof(uint16_t)*3; + memcpy(rudder, p->rudder, sizeof(p->rudder)); + return sizeof(p->rudder); } /** @@ -156,11 +214,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_me * * @return Tail gyro mode/gain setpoints: heading hold, rate mode */ -static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* gyro) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*2); - return sizeof(uint16_t)*2; + memcpy(gyro, p->gyro, sizeof(p->gyro)); + return sizeof(p->gyro); } /** @@ -168,11 +227,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_mess * * @return Pitch curve setpoints (every 25%) */ -static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* pitch) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; + memcpy(pitch, p->pitch, sizeof(p->pitch)); + return sizeof(p->pitch); } /** @@ -180,11 +240,12 @@ static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_mes * * @return Throttle curve setpoints (every 25%) */ -static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* r_data) +static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* throttle) { + mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; - memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2+sizeof(uint16_t)*5, sizeof(uint16_t)*5); - return sizeof(uint16_t)*5; + memcpy(throttle, p->throttle, sizeof(p->throttle)); + return sizeof(p->throttle); } /** @@ -195,10 +256,5 @@ static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_ */ static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* msg, mavlink_radio_calibration_t* radio_calibration) { - mavlink_msg_radio_calibration_get_aileron(msg, radio_calibration->aileron); - mavlink_msg_radio_calibration_get_elevator(msg, radio_calibration->elevator); - mavlink_msg_radio_calibration_get_rudder(msg, radio_calibration->rudder); - mavlink_msg_radio_calibration_get_gyro(msg, radio_calibration->gyro); - mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); - mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); + memcpy( radio_calibration, msg->payload, sizeof(mavlink_radio_calibration_t)); } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h deleted file mode 100644 index 64202ffe2..000000000 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_request_rc_channels.h +++ /dev/null @@ -1,101 +0,0 @@ -// MESSAGE REQUEST_RC_CHANNELS PACKING - -#define MAVLINK_MSG_ID_REQUEST_RC_CHANNELS 221 - -typedef struct __mavlink_request_rc_channels_t -{ - uint8_t enabled; ///< True: start sending data; False: stop sending data - -} mavlink_request_rc_channels_t; - - - -/** - * @brief Pack a request_rc_channels 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 enabled True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message(msg, system_id, component_id, i); -} - -/** - * @brief Pack a request_rc_channels message - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enabled True: start sending data; False: stop sending data - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_request_rc_channels_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t enabled) -{ - uint16_t i = 0; - msg->msgid = MAVLINK_MSG_ID_REQUEST_RC_CHANNELS; - - i += put_uint8_t_by_index(enabled, i, msg->payload); // True: start sending data; False: stop sending data - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); -} - -/** - * @brief Encode a request_rc_channels struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param request_rc_channels C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_request_rc_channels_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_request_rc_channels_t* request_rc_channels) -{ - return mavlink_msg_request_rc_channels_pack(system_id, component_id, msg, request_rc_channels->enabled); -} - -/** - * @brief Send a request_rc_channels message - * @param chan MAVLink channel to send the message - * - * @param enabled True: start sending data; False: stop sending data - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_request_rc_channels_send(mavlink_channel_t chan, uint8_t enabled) -{ - mavlink_message_t msg; - mavlink_msg_request_rc_channels_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, enabled); - mavlink_send_uart(chan, &msg); -} - -#endif -// MESSAGE REQUEST_RC_CHANNELS UNPACKING - -/** - * @brief Get field enabled from request_rc_channels message - * - * @return True: start sending data; False: stop sending data - */ -static inline uint8_t mavlink_msg_request_rc_channels_get_enabled(const mavlink_message_t* msg) -{ - return (uint8_t)(msg->payload)[0]; -} - -/** - * @brief Decode a request_rc_channels message into a struct - * - * @param msg The message to decode - * @param request_rc_channels C-struct to decode the message contents into - */ -static inline void mavlink_msg_request_rc_channels_decode(const mavlink_message_t* msg, mavlink_request_rc_channels_t* request_rc_channels) -{ - request_rc_channels->enabled = mavlink_msg_request_rc_channels_get_enabled(msg); -} diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h index 50e8f7d02..5546cdc5f 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -1,6 +1,8 @@ // MESSAGE UALBERTA_SYS_STATUS PACKING #define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222 +#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN 3 +#define MAVLINK_MSG_222_LEN 3 typedef struct __mavlink_ualberta_sys_status_t { @@ -10,8 +12,6 @@ typedef struct __mavlink_ualberta_sys_status_t } mavlink_ualberta_sys_status_t; - - /** * @brief Pack a ualberta_sys_status message * @param system_id ID of this system @@ -25,14 +25,14 @@ typedef struct __mavlink_ualberta_sys_status_t */ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - uint16_t i = 0; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message(msg, system_id, component_id, i); + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } /** @@ -48,14 +48,14 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, u */ static inline uint16_t mavlink_msg_ualberta_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 pilot) { - uint16_t i = 0; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM - i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } /** @@ -80,12 +80,61 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, * @param pilot Pilot mode, see UALBERTA_PILOT_MODE */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { mavlink_message_t msg; - mavlink_msg_ualberta_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, pilot); - mavlink_send_uart(chan, &msg); + uint16_t checksum; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg.payload[0]; + + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + + msg.STX = MAVLINK_STX; + msg.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; + msg.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + msg.sysid = mavlink_system.sysid; + msg.compid = mavlink_system.compid; + msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; + checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); + msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_msg(chan, &msg); +} + +#endif + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) +{ + mavlink_header_t hdr; + mavlink_ualberta_sys_status_t payload; + uint16_t checksum; + mavlink_ualberta_sys_status_t *p = &payload; + + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; + hdr.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); } #endif @@ -98,7 +147,8 @@ static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload)[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->mode); } /** @@ -108,7 +158,8 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_mes */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t))[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->nav_mode); } /** @@ -118,7 +169,8 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink */ static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_message_t* msg) { - return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; + mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; + return (uint8_t)(p->pilot); } /** @@ -129,7 +181,5 @@ static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_me */ static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_t* msg, mavlink_ualberta_sys_status_t* ualberta_sys_status) { - ualberta_sys_status->mode = mavlink_msg_ualberta_sys_status_get_mode(msg); - ualberta_sys_status->nav_mode = mavlink_msg_ualberta_sys_status_get_nav_mode(msg); - ualberta_sys_status->pilot = mavlink_msg_ualberta_sys_status_get_pilot(msg); + memcpy( ualberta_sys_status, msg->payload, sizeof(mavlink_ualberta_sys_status_t)); } diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index b492ff62d..b957f6d73 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Wednesday, July 27 2011, 14:17 UTC + * Generated on Sunday, July 31 2011, 15:12 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -30,7 +30,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Available autopilot modes for ualberta uav */ +/** @brief Available autopilot modes for ualberta uav */ enum UALBERTA_AUTOPILOT_MODE { MODE_MANUAL_DIRECT=0, /* */ @@ -41,7 +41,7 @@ enum UALBERTA_AUTOPILOT_MODE UALBERTA_AUTOPILOT_MODE_ENUM_END }; -/** @brief Navigation filter mode */ +/** @brief Navigation filter mode */ enum UALBERTA_NAV_MODE { NAV_AHRS_INIT=0, @@ -51,7 +51,7 @@ enum UALBERTA_NAV_MODE UALBERTA_NAV_MODE_ENUM_END }; -/** @brief Mode currently commanded by pilot */ +/** @brief Mode currently commanded by pilot */ enum UALBERTA_PILOT_MODE { PILOT_MANUAL=0, /* */ @@ -66,13 +66,6 @@ enum UALBERTA_PILOT_MODE #include "./mavlink_msg_nav_filter_bias.h" #include "./mavlink_msg_radio_calibration.h" #include "./mavlink_msg_ualberta_sys_status.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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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, 32, 42, 3, 0, 0, 0, 0, 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 diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 5df9ba827..de21876a8 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -1,892 +1,1131 @@ - + -2 - - - - Commands to be executed by the MAV. They can be executed on user request, + 3 + + + Micro air vehicle / autopilot classes. This identifies the individual model. + + Generic autopilot, full support for everything + + + PIXHAWK autopilot, http://pixhawk.ethz.ch + + + SLUGS autopilot, http://slugsuav.soe.ucsc.edu + + + ArduPilotMega / ArduCopter, http://diydrones.com + + + OpenPilot, http://openpilot.org + + + Generic autopilot only supporting simple waypoints + + + Generic autopilot supporting waypoints and other simple navigation commands + + + Generic autopilot supporting the full mission command set + + + No valid autopilot, e.g. a GCS or other MAVLink component + + + PPZ UAV - http://nongnu.org/paparazzi + + + UAV Dev Board + + + FlexiPilot + + + + + + System is in undefined state + + + Motors are blocked, system is safe + + + System is allowed to be active, under manual (RC) control + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is allowed to be active, under autonomous control and navigation + + + Generic test mode, for custom use + + + Generic test mode, for custom use + + + Generic test mode, for custom use + + + System is ready, motors are unblocked, but controllers are inactive + + + System is blocked, only RC valued are read and reported back + + + + + System is currently on ground. + + + System is during liftoff, not in normal navigation mode yet. + + + System is holding its current position (rotorcraft or rover / boat). + + + System is navigating towards the next waypoint. + + + System is flying at a defined course and speed. + + + System is return straight to home position. + + + System is landing. + + + System lost its position input and is in attitude / flight stabilization only. + + + System is loitering in wait position. DO NOT USE THIS FOR WAYPOINT LOITER! + + + + + Uninitialized system, state is unknown. + + + System is booting up. + + + System is calibrating and not flight-ready. + + + System is grounded and on standby. It can be launched any time. + + + System is active and might be already airborne. Motors are engaged. + + + System is in a non-normal flight mode. It can however still navigate. + + + System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. + + + System just initialized its power-down sequence, will shut down now. + + + + 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. + + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + + + 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). + + + Point toward next waypoint. + + + Point toward given waypoint. + + + Point toward fixed location. + + + Point toward of given id. + + + + + Generic micro air vehicle. + + + Fixed wing aircraft. + + + Quadrotor + + + Coaxial helicopter + + + Normal helicopter with tail rotor. + + + Ground installation + + + Operator control unit / ground control station + + + Airship, controlled + + + Free balloon, uncontrolled + + + Rocket + + + Ground rover + + + Surface vessel, boat, ship + + + Submarine + + + Hexarotor + + + Octorotor + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Global coordinate frame, WGS84 coordinate system. + + + Local coordinate frame, Z-up (x: north, y: east, z: down). + + + NOT a coordinate frame, indicates a mission command. + + + Global coordinate frame, WGS84 coordinate system, relative altitude over ground. + + + Local coordinate frame, Z-down (x: east, y: north, z: up) + + + + + + + + + + + + + + + + + + + + + + + + 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. - - 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 - - - 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 - - - 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 - - - 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 - - - Return to launch location - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Land at location - Empty - Empty - Empty - Desired yaw angle. - Latitude - Longitude - Altitude - - - 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 - - - - Sets the region of interest (ROI) for a sensor set or the + + 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 + + + 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 + + + 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 + + + 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 + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location + Empty + Empty + Empty + Desired yaw angle. + Latitude + Longitude + Altitude + + + 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 + + + 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 - - - - 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 - - - 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 - - - Delay mission state machine. - Delay in seconds (decimal) - Empty - Empty - Empty - Empty - Empty - Empty - - - Ascend/descend at rate. Delay mission state machine until desired altitude reached. - Descent / Ascend rate (m/s) - Empty - Empty - Empty - Empty - Empty - Finish Altitude - - - Delay mission state machine until within desired distance of next NAV point. - Distance (meters) - Empty - Empty - Empty - Empty - Empty - Empty - - - 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 - - - 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 - - - Set system mode. - Mode, as defined by ENUM MAV_MODE - Empty - Empty - Empty - Empty - Empty - Empty - - - 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 - - - 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 - - - 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 - - - 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 - - - Set a relay to a condition. - Relay number - Setting (1=on, 0=off, others possible depending on system hardware) - Empty - Empty - Empty - Empty - Empty - - - 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 - - - Set a servo to a desired PWM value. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Empty - Empty - Empty - Empty - Empty - - - 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 - - - Control onboard camera system. - 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 - - - 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 - - - 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 - - - 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 - - - - 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. - - Enable all data streams - Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. - Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS - Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW - Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. - Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. - Dependent on the autopilot - Dependent on the autopilot - Dependent on the autopilot - - - - 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). - - Point toward next waypoint. - Point toward given waypoint. - Point toward fixed location. - Point toward of given id. - - - - - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - - - The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. - The onboard software version - - - - The system time is the time of the master clock, typically the computer clock of the main onboard computer. - Timestamp of the master clock in microseconds since UNIX epoch. - - - - A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. - PING sequence - 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 - 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - Unix timestamp in microseconds - - - - UTC date and time from GPS module - GPS UTC date ddmmyy - GPS UTC time hhmmss - - - - Request to control this MAV - System the GCS requests control for - 0: request control of this MAV, 1: Release control of this MAV - 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. - 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 "!?,.-" - - - - Accept / deny control of this MAV - ID of the GCS this message - 0: request control of this MAV, 1: Release control of this MAV - 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - - - Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. - key - - - - This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The action id - 0: Action DENIED, 1: Action executed - - - - An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The system executing the action - The component executing the action - The action id - - - - Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. - The system setting the mode - The new mode - - - - Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. - The system setting the mode - The new navigation mode - - - - Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. - System ID - Component ID - Onboard parameter id - Parameter index. Send -1 to use the param ID field as identifier - - - - Request all parameters of this component. After his request, all parameters are emitted. - System ID - Component ID - - - - Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id - Onboard parameter value - Total number of onboard parameters - Index of this onboard parameter - - - - Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. - System ID - Component ID - Onboard parameter id - Onboard parameter value - - - - The global position, as returned by the Global Positioning System (GPS). This is + 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 + + + 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 + + + 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 + + + Delay mission state machine. + Delay in seconds (decimal) + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend at rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate (m/s) + Empty + Empty + Empty + Empty + Empty + Finish Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance (meters) + Empty + Empty + Empty + Empty + Empty + Empty + + + 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 + + + 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 + + + Set system mode. + Mode, as defined by ENUM MAV_MODE + Empty + Empty + Empty + Empty + Empty + Empty + + + 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 + + + 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 + + + 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 + + + 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 + + + Set a relay to a condition. + Relay number + Setting (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + 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 + + + Set a servo to a desired PWM value. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Empty + Empty + Empty + Empty + Empty + + + 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 + + + Control onboard camera system. + 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 + + + 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 + + + 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 + + + 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 + + + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + + The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. + The onboard software version + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp of the master clock in microseconds since UNIX epoch. + + + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. + PING sequence + 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 + 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + Unix timestamp in microseconds + + + UTC date and time from GPS module + GPS UTC date ddmmyy + GPS UTC time hhmmss + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 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. + 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 "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The action id + 0: Action DENIED, 1: Action executed + + + An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The system executing the action + The component executing the action + The action id + + + Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new mode + + + Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. + The system setting the mode + The new navigation mode + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id + Parameter index. Send -1 to use the param ID field as identifier + + + Request all parameters of this component. After his request, all parameters are emitted. + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. + Onboard parameter id + Onboard parameter value + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. + System ID + Component ID + Onboard parameter id + Onboard parameter value + + + The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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. - Latitude in 1E7 degrees - Longitude in 1E7 degrees - Altitude in 1E3 meters (millimeters) - GPS HDOP - GPS VDOP - GPS ground speed (m/s) - Compass heading in degrees, 0..360 degrees - - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (mg) - Y acceleration (mg) - Z acceleration (mg) - Angular speed around X axis (millirad /sec) - Angular speed around Y axis (millirad /sec) - Angular speed around Z axis (millirad /sec) - X Magnetic field (milli tesla) - Y Magnetic field (milli tesla) - Z Magnetic field (milli tesla) - - - - The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. - Number of satellites visible - Global satellite ID - 0: Satellite not used, 1: used for localization - Elevation (0: right on top of receiver, 90: on the horizon) of satellite - Direction of satellite, 0: 0 deg, 255: 360 deg. - Signal to noise ratio of satellite - - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (raw) - Y acceleration (raw) - Z acceleration (raw) - Angular speed around X axis (raw) - Angular speed around Y axis (raw) - Angular speed around Z axis (raw) - X Magnetic field (raw) - Y Magnetic field (raw) - Z Magnetic field (raw) - - - - The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (raw) - Differential pressure 1 (raw) - Differential pressure 2 (raw) - Raw Temperature measurement (raw) - - - - The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (hectopascal) - Differential pressure 1 (hectopascal) - Temperature measurement (0.01 degrees celsius) - - - - - The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Roll angle (rad) - Pitch angle (rad) - Yaw angle (rad) - Roll angular speed (rad/s) - Pitch angular speed (rad/s) - Yaw angular speed (rad/s) - - - - The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X Position - Y Position - Z Position - X Speed - Y Speed - Z Speed - - - - - The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since unix epoch) - Latitude, in degrees - Longitude, in degrees - Absolute altitude, in meters - X Speed (in Latitude direction, positive: going north) - Y Speed (in Longitude direction, positive: going east) - Z Speed (in Altitude direction, positive: going up) - - - - The global position, as returned by the Global Positioning System (GPS). This is + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 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. + Latitude in 1E7 degrees + Longitude in 1E7 degrees + Altitude in 1E3 meters (millimeters) + GPS HDOP + GPS VDOP + GPS ground speed (m/s) + Compass heading in degrees, 0..360 degrees + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + Angular speed around X axis (millirad /sec) + Angular speed around Y axis (millirad /sec) + Angular speed around Z axis (millirad /sec) + X Magnetic field (milli tesla) + Y Magnetic field (milli tesla) + Z Magnetic field (milli tesla) + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (raw) + Differential pressure 1 (raw) + Differential pressure 2 (raw) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (hectopascal) + Differential pressure 1 (hectopascal) + Temperature measurement (0.01 degrees celsius) + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since unix epoch) + Latitude, in degrees + Longitude, in degrees + Absolute altitude, in meters + X Speed (in Latitude direction, positive: going north) + Y Speed (in Longitude direction, positive: going east) + Z Speed (in Altitude direction, positive: going up) + + + The global position, as returned by the Global Positioning System (GPS). This is NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 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. - Latitude in degrees - Longitude in degrees - Altitude in meters - GPS HDOP - GPS VDOP - GPS ground speed - Compass heading in degrees, 0..360 degrees - - - - The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. - System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - Navigation mode, see MAV_NAV_MODE ENUM - System status flag, see MAV_STATUS ENUM - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Battery voltage, in millivolts (1 = 1 millivolt) - Remaining battery energy: (0%: 0, 100%: 1000) - Dropped packets (packets that were corrupted on reception on the MAV) - - - - The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - RC channel 1 value, in microseconds - RC channel 2 value, in microseconds - RC channel 3 value, in microseconds - RC channel 4 value, in microseconds - RC channel 5 value, in microseconds - RC channel 6 value, in microseconds - RC channel 7 value, in microseconds - RC channel 8 value, in microseconds - Receive signal strength indicator, 0: 0%, 255: 100% - - - - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - Receive signal strength indicator, 0: 0%, 255: 100% - - - - The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. - Servo output 1 value, in microseconds - Servo output 2 value, in microseconds - Servo output 3 value, in microseconds - Servo output 4 value, in microseconds - Servo output 5 value, in microseconds - Servo output 6 value, in microseconds - Servo output 7 value, in microseconds - Servo output 8 value, in microseconds - - - - Message encoding a waypoint. This message is emitted to announce + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 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. + Latitude in degrees + Longitude in degrees + Altitude in meters + GPS HDOP + GPS VDOP + GPS ground speed + Compass heading in degrees, 0..360 degrees + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + Navigation mode, see MAV_NAV_MODE ENUM + System status flag, see MAV_STATUS ENUM + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Battery voltage, in millivolts (1 = 1 millivolt) + Remaining battery energy: (0%: 0, 100%: 1000) + Dropped packets (packets that were corrupted on reception on the MAV) + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + + The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + Receive signal strength indicator, 0: 0%, 255: 100% + + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Servo output 1 value, in microseconds + Servo output 2 value, in microseconds + Servo output 3 value, in microseconds + Servo output 4 value, in microseconds + Servo output 5 value, in microseconds + Servo output 6 value, in microseconds + Servo output 7 value, in microseconds + Servo output 8 value, in microseconds + + + Message encoding a waypoint. This message is emitted to announce the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed - System ID - Component ID - Sequence - The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - false:0, true:1 - autocontinue to next wp - PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - 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. - PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - PARAM5 / local: x position, global: latitude - PARAM6 / y position: global: longitude - PARAM7 / z position: global: altitude - - - - Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. - System ID - Component ID - Sequence - - - - Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). - System ID - Component ID - Sequence - - - - Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. - Sequence - - - - Request the overall list of waypoints from the system/component. - System ID - Component ID - - - - This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. - System ID - Component ID - Number of Waypoints in the Sequence - - - - Delete all waypoints at once. - System ID - Component ID - - - - A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. - Sequence - - - - Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). - System ID - Component ID - 0: OK, 1: Error - - - - As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. - System ID - Component ID - global position * 1E7 - global position * 1E7 - global position * 1000 - - - - Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position - Latitude (WGS84), expressed as * 1E7 - Longitude (WGS84), expressed as * 1E7 - Altitude(WGS84), expressed as * 1000 - - - - Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. - System ID - Component ID - x position - y position - z position - Desired yaw angle - - - - Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. - x position - y position - z position - Desired yaw angle - - - - Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - Attitude estimation health: 0: poor, 255: excellent - 0: Attitude control disabled, 1: enabled - 0: X, Y position control disabled, 1: enabled - 0: Z position control disabled, 1: enabled - 0: Yaw angle control disabled, 1: enabled - - - - Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. - System ID - Component ID - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - - Read out the safety zone the MAV currently assumes. - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - - The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Attitude roll: -128: -100%, 127: +100% - Attitude pitch: -128: -100%, 127: +100% - Attitude yaw: -128: -100%, 127: +100% - Attitude thrust: -128: -100%, 127: +100% - - - - The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Position x: -128: -100%, 127: +100% - Position y: -128: -100%, 127: +100% - Position z: -128: -100%, 127: +100% - Position yaw: -128: -100%, 127: +100% - - - - Outputs of the APM navigation controller. The primary use of this message is to check the response and signs + System ID + Component ID + Sequence + The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + false:0, true:1 + autocontinue to next wp + PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + 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. + PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + PARAM5 / local: x position, global: latitude + PARAM6 / y position: global: longitude + PARAM7 / z position: global: altitude + + + Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. + System ID + Component ID + Sequence + + + Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. + Sequence + + + Request the overall list of waypoints from the system/component. + System ID + Component ID + + + This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of Waypoints in the Sequence + + + Delete all waypoints at once. + System ID + Component ID + + + A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + 0: OK, 1: Error + + + As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Component ID + global position * 1E7 + global position * 1E7 + global position * 1000 + + + Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position + Latitude (WGS84), expressed as * 1E7 + Longitude (WGS84), expressed as * 1E7 + Altitude(WGS84), expressed as * 1000 + + + Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. + System ID + Component ID + x position + y position + z position + Desired yaw angle + + + Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. + x position + y position + z position + Desired yaw angle + + + Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + Attitude estimation health: 0: poor, 255: excellent + 0: Attitude control disabled, 1: enabled + 0: X, Y position control disabled, 1: enabled + 0: Z position control disabled, 1: enabled + 0: Yaw angle control disabled, 1: enabled + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. + 1: enabled, 0: disabled + Attitude roll: -128: -100%, 127: +100% + Attitude pitch: -128: -100%, 127: +100% + Attitude yaw: -128: -100%, 127: +100% + Attitude thrust: -128: -100%, 127: +100% + + + The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. + 1: enabled, 0: disabled + Position x: -128: -100%, 127: +100% + Position y: -128: -100%, 127: +100% + Position z: -128: -100%, 127: +100% + Position yaw: -128: -100%, 127: +100% + + + Outputs of the APM navigation controller. The primary use of this message is to check the response and signs of the controller before actual flight and to assist with tuning controller parameters - Current desired roll in degrees - Current desired pitch in degrees - Current desired heading in degrees - Bearing to current waypoint/target in degrees - Distance to active waypoint in meters - Current altitude error in meters - Current airspeed error in meters/second - Current crosstrack error on x-y plane in meters - - - - The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. - x position error - y position error - z position error - roll error (radians) - pitch error (radians) - yaw error (radians) - x velocity - y velocity - z velocity - - - - The system setting the altitude - The new altitude in meters - - - - The target requested to send the message stream. - The target requested to send the message stream. - The ID of the requested message type - The requested interval between two messages of this type - 1 to start sending, 0 to stop sending. - - - - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - - The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) - Latitude, expressed as * 1E7 - Longitude, expressed as * 1E7 - Altitude in meters, expressed as * 1000 (millimeters) - Ground X Speed (Latitude), expressed as m/s * 100 - Ground Y Speed (Longitude), expressed as m/s * 100 - Ground Z Speed (Altitude), expressed as m/s * 100 - - - - Metrics typically displayed on a HUD for fixed wing aircraft - Current airspeed in m/s - Current ground speed in m/s - Current heading in degrees, in compass units (0..360, 0=north) - Current throttle setting in integer percent, 0 to 100 - Current altitude (MSL), in meters - Current climb rate in meters/second - - - - Send a command with up to four parameters to the MAV - System which should execute the command - Component which should execute the command, 0 for all components - Command ID, as defined by MAV_CMD enum. - 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - Parameter 1, as defined by MAV_CMD enum. - Parameter 2, as defined by MAV_CMD enum. - Parameter 3, as defined by MAV_CMD enum. - Parameter 4, as defined by MAV_CMD enum. - - - - Report status of a command. Includes feedback wether the command was executed - Current airspeed in m/s - 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - - - - - - - - - Name - Timestamp - x - y - z - - - - Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Floating point value - - - - Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Signed integer value - - - - Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). - Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character - - - - Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. - index of debug variable - DEBUG value - - - + Current desired roll in degrees + Current desired pitch in degrees + Current desired heading in degrees + Bearing to current waypoint/target in degrees + Distance to active waypoint in meters + Current altitude error in meters + Current airspeed error in meters/second + Current crosstrack error on x-y plane in meters + + + The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. + x position error + y position error + z position error + roll error (radians) + pitch error (radians) + yaw error (radians) + x velocity + y velocity + z velocity + + + The system setting the altitude + The new altitude in meters + + + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested message type + The requested interval between two messages of this type + 1 to start sending, 0 to stop sending. + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + + + Metrics typically displayed on a HUD for fixed wing aircraft + Current airspeed in m/s + Current ground speed in m/s + Current heading in degrees, in compass units (0..360, 0=north) + Current throttle setting in integer percent, 0 to 100 + Current altitude (MSL), in meters + Current climb rate in meters/second + + + Send a command with up to four parameters to the MAV + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + + + Report status of a command. Includes feedback wether the command was executed + Current airspeed in m/s + 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + + + Name + Timestamp + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status, 0 = info message, 255 = critical fault + Status text message, without null termination character + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + index of debug variable + DEBUG value + + diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index e2a99a738..46f18b154 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -175,8 +175,8 @@ Watchdog ID Process ID - Process name - Process arguments + Process name + Process arguments Timeout (seconds) @@ -199,7 +199,7 @@ 0: Pattern, 1: Letter Confidence of detection - Pattern file name + Pattern file name Accepted as true detection, 0 no, 1 yes @@ -214,7 +214,7 @@ X Position Y Position Z Position - POI name + POI name @@ -231,7 +231,7 @@ X2 Position Y2 Position Z2 Position - POI connection name + POI connection name @@ -254,7 +254,7 @@ Orientation assignment 0: false, 1:true Size in pixels Orientation - Descriptor + Descriptor Harris operator response at this location diff --git a/thirdParty/mavlink/missionlib/testing/.gitignore b/thirdParty/mavlink/missionlib/testing/.gitignore new file mode 100644 index 000000000..4b4c3f6c4 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/.gitignore @@ -0,0 +1,2 @@ +*.o +udptest diff --git a/thirdParty/mavlink/missionlib/testing/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c new file mode 100644 index 000000000..37a8a7848 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/udp.c @@ -0,0 +1,213 @@ +/******************************************************************************* + Copyright (C) 2010 Bryan Godbolt godbolt ( a t ) ualberta.ca + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ +/* + This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets + cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from + qgroundcontrol are printed by this program along with the heartbeats. + + + I compiled this program sucessfully on Ubuntu 10.04 with the following command + + gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c + + the rt library is needed for the clock_gettime on linux + */ +/* These headers are for QNX, but should all be standard on unix/linux */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if (defined __QNX__) | (defined __QNXNTO__) +/* QNX specific headers */ +#include +#else +/* Linux / MacOS POSIX timer headers */ +#include +#include +#include +#endif + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +uint64_t microsSinceEpoch(); + +int main(int argc, char* argv[]) +{ + + char help[] = "--help"; + + + char target_ip[100]; + + float position[6] = {}; + int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + struct sockaddr_in gcAddr; + struct sockaddr_in locAddr; + //struct sockaddr_in fromAddr; + uint8_t buf[BUFFER_LENGTH]; + ssize_t recsize; + socklen_t fromlen; + int bytes_sent; + mavlink_message_t msg; + uint16_t len; + int i = 0; + //int success = 0; + unsigned int temp = 0; + + // Check if --help flag was used + if ((argc == 2) && (strcmp(argv[1], help) == 0)) + { + printf("\n"); + printf("\tUsage:\n\n"); + printf("\t"); + printf("%s", argv[0]); + printf(" \n"); + printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); + exit(EXIT_FAILURE); + } + + + // Change the target ip if parameter was given + strcpy(target_ip, "127.0.0.1"); + if (argc == 2) + { + strcpy(target_ip, argv[1]); + } + + + memset(&locAddr, 0, sizeof(locAddr)); + locAddr.sin_family = AF_INET; + locAddr.sin_addr.s_addr = INADDR_ANY; + locAddr.sin_port = htons(14551); + + /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ + if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) + { + perror("error bind failed"); + close(sock); + exit(EXIT_FAILURE); + } + + /* Attempt to make it non blocking */ + if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); + close(sock); + exit(EXIT_FAILURE); + } + + + memset(&gcAddr, 0, sizeof(gcAddr)); + gcAddr.sin_family = AF_INET; + gcAddr.sin_addr.s_addr = inet_addr(target_ip); + gcAddr.sin_port = htons(14550); + + + + for (;;) + { + + /*Send Heartbeat */ + mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send Status */ + mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + /* Send Local Position */ + mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), + position[0], position[1], position[2], + position[3], position[4], position[5]); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send attitude */ + mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + + memset(buf, 0, BUFFER_LENGTH); + recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); + if (recsize > 0) + { + // Something received - print out all bytes and parse packet + mavlink_message_t msg; + mavlink_status_t status; + + printf("Bytes Received: %d\nDatagram: ", (int)recsize); + for (i = 0; i < recsize; ++i) + { + temp = buf[i]; + printf("%02x ", (unsigned char)temp); + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // Packet received + printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + sleep(1); // Sleep one second + } +} + + +/* QNX timer version */ +#if (defined __QNX__) | (defined __QNXNTO__) +uint64_t microsSinceEpoch() +{ + + struct timespec time; + + uint64_t micros = 0; + + clock_gettime(CLOCK_REALTIME, &time); + micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; + + return micros; +} +#else +uint64_t microsSinceEpoch() +{ + + struct timeval tv; + + uint64_t micros = 0; + + gettimeofday(&tv, NULL); + micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + + return micros; +} +#endif \ No newline at end of file -- 2.22.0