From 411875fde002fa405ae2bdfa2555081802385f3a Mon Sep 17 00:00:00 2001 From: LM Date: Thu, 1 Sep 2011 09:44:04 +0200 Subject: [PATCH] Updated packaged files --- .../include/ardupilotmega/ardupilotmega.h | 6 +- .../mavlink_msg_sensor_offsets.h | 172 +- .../mavlink_msg_set_mag_offsets.h | 95 +- .../mavlink/include/ardupilotmega/testsuite.h | 91 +- .../mavlink/include/ardupilotmega/version.h | 2 +- thirdParty/mavlink/include/checksum.h | 17 + thirdParty/mavlink/include/common/common.h | 6 +- .../include/common/mavlink_msg_attitude.h | 109 +- .../include/common/mavlink_msg_auth_key.h | 43 +- .../mavlink_msg_change_operator_control.h | 78 +- .../mavlink_msg_change_operator_control_ack.h | 65 +- .../include/common/mavlink_msg_command_ack.h | 54 +- .../include/common/mavlink_msg_command_long.h | 161 +- .../common/mavlink_msg_command_short.h | 120 +- .../include/common/mavlink_msg_data_stream.h | 65 +- .../include/common/mavlink_msg_debug.h | 65 +- .../include/common/mavlink_msg_debug_vect.h | 89 +- .../common/mavlink_msg_extended_message.h | 65 +- .../common/mavlink_msg_global_position.h | 276 ++ .../common/mavlink_msg_global_position_int.h | 120 +- ...mavlink_msg_global_position_setpoint_int.h | 78 +- .../common/mavlink_msg_gps_global_origin.h | 65 +- .../include/common/mavlink_msg_gps_raw.h | 342 +++ .../include/common/mavlink_msg_gps_raw_int.h | 150 +- .../mavlink_msg_gps_set_global_origin.h | 232 ++ .../include/common/mavlink_msg_gps_status.h | 100 +- .../include/common/mavlink_msg_heartbeat.h | 106 +- .../include/common/mavlink_msg_hil_controls.h | 161 +- .../common/mavlink_msg_hil_rc_inputs_raw.h | 194 +- .../include/common/mavlink_msg_hil_state.h | 216 +- .../common/mavlink_msg_local_position.h | 109 +- .../mavlink_msg_local_position_setpoint.h | 78 +- .../common/mavlink_msg_manual_control.h | 133 +- .../include/common/mavlink_msg_memory_vect.h | 78 +- .../common/mavlink_msg_named_value_float.h | 67 +- .../common/mavlink_msg_named_value_int.h | 67 +- .../mavlink_msg_nav_controller_output.h | 120 +- .../include/common/mavlink_msg_optical_flow.h | 106 +- .../common/mavlink_msg_param_request_list.h | 54 +- .../common/mavlink_msg_param_request_read.h | 78 +- .../include/common/mavlink_msg_param_set.h | 89 +- .../include/common/mavlink_msg_param_value.h | 89 +- .../mavlink/include/common/mavlink_msg_ping.h | 78 +- .../common/mavlink_msg_position_target.h | 210 ++ .../include/common/mavlink_msg_raw_imu.h | 150 +- .../include/common/mavlink_msg_raw_pressure.h | 95 +- .../common/mavlink_msg_rc_channels_override.h | 150 +- .../common/mavlink_msg_rc_channels_raw.h | 161 +- .../common/mavlink_msg_rc_channels_scaled.h | 161 +- .../common/mavlink_msg_request_data_stream.h | 95 +- ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 95 +- ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 95 +- .../common/mavlink_msg_safety_allowed_area.h | 109 +- .../mavlink_msg_safety_set_allowed_area.h | 133 +- .../include/common/mavlink_msg_scaled_imu.h | 150 +- .../common/mavlink_msg_scaled_pressure.h | 78 +- .../common/mavlink_msg_servo_output_raw.h | 150 +- .../common/mavlink_msg_set_flight_mode.h | 166 ++ ...ink_msg_set_global_position_setpoint_int.h | 78 +- .../mavlink_msg_set_gps_global_origin.h | 78 +- .../mavlink_msg_set_local_position_setpoint.h | 106 +- .../include/common/mavlink_msg_set_mode.h | 65 +- ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 106 +- .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 106 +- .../common/mavlink_msg_set_safety_mode.h | 166 ++ .../common/mavlink_msg_state_correction.h | 133 +- .../include/common/mavlink_msg_statustext.h | 56 +- .../include/common/mavlink_msg_sys_status.h | 245 +- .../include/common/mavlink_msg_system_time.h | 54 +- .../common/mavlink_msg_system_time_utc.h | 166 ++ .../include/common/mavlink_msg_vfr_hud.h | 106 +- .../include/common/mavlink_msg_waypoint.h | 194 +- .../include/common/mavlink_msg_waypoint_ack.h | 65 +- .../common/mavlink_msg_waypoint_clear_all.h | 54 +- .../common/mavlink_msg_waypoint_count.h | 65 +- .../common/mavlink_msg_waypoint_current.h | 43 +- .../common/mavlink_msg_waypoint_reached.h | 43 +- .../common/mavlink_msg_waypoint_request.h | 65 +- .../mavlink_msg_waypoint_request_list.h | 54 +- .../common/mavlink_msg_waypoint_set_current.h | 65 +- thirdParty/mavlink/include/common/testsuite.h | 2457 ++++++++++++++--- thirdParty/mavlink/include/common/version.h | 2 +- thirdParty/mavlink/include/mavlink_helpers.h | 52 +- thirdParty/mavlink/include/mavlink_types.h | 36 +- .../include/minimal/mavlink_msg_heartbeat.h | 120 +- thirdParty/mavlink/include/minimal/minimal.h | 4 + .../mavlink/include/minimal/testsuite.h | 47 +- thirdParty/mavlink/include/minimal/version.h | 4 + .../pixhawk/mavlink_msg_attitude_control.h | 133 +- .../pixhawk/mavlink_msg_brief_feature.h | 122 +- .../mavlink_msg_data_transmission_handshake.h | 95 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 56 +- .../pixhawk/mavlink_msg_image_available.h | 293 +- .../mavlink_msg_image_trigger_control.h | 43 +- .../pixhawk/mavlink_msg_image_triggered.h | 172 +- .../include/pixhawk/mavlink_msg_marker.h | 109 +- .../pixhawk/mavlink_msg_pattern_detected.h | 78 +- .../pixhawk/mavlink_msg_point_of_interest.h | 122 +- ...mavlink_msg_point_of_interest_connection.h | 155 +- .../mavlink_msg_position_control_offset_set.h | 106 +- .../mavlink_msg_position_control_setpoint.h | 95 +- ...avlink_msg_position_control_setpoint_set.h | 109 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 109 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 106 +- .../mavlink_msg_vicon_position_estimate.h | 109 +- .../mavlink_msg_vision_position_estimate.h | 109 +- .../mavlink_msg_vision_speed_estimate.h | 78 +- .../pixhawk/mavlink_msg_watchdog_command.h | 78 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 54 +- .../mavlink_msg_watchdog_process_info.h | 89 +- .../mavlink_msg_watchdog_process_status.h | 106 +- thirdParty/mavlink/include/pixhawk/pixhawk.h | 8 +- .../mavlink/include/pixhawk/testsuite.h | 881 +++++- thirdParty/mavlink/include/pixhawk/version.h | 6 +- thirdParty/mavlink/include/protocol.h | 272 +- .../include/slugs/mavlink_msg_air_data.h | 65 +- .../include/slugs/mavlink_msg_cpu_load.h | 65 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 54 +- .../include/slugs/mavlink_msg_data_log.h | 106 +- .../include/slugs/mavlink_msg_diagnostic.h | 106 +- .../include/slugs/mavlink_msg_gps_date_time.h | 109 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 78 +- .../include/slugs/mavlink_msg_sensor_bias.h | 106 +- .../include/slugs/mavlink_msg_slugs_action.h | 65 +- .../slugs/mavlink_msg_slugs_navigation.h | 133 +- thirdParty/mavlink/include/slugs/slugs.h | 4 + thirdParty/mavlink/include/slugs/testsuite.h | 371 ++- thirdParty/mavlink/include/slugs/version.h | 4 + .../include/test/mavlink_msg_test_types.h | 292 +- thirdParty/mavlink/include/test/test.h | 4 + thirdParty/mavlink/include/test/testsuite.h | 61 +- thirdParty/mavlink/include/test/version.h | 4 + .../ualberta/mavlink_msg_nav_filter_bias.h | 109 +- .../ualberta/mavlink_msg_radio_calibration.h | 98 +- .../mavlink_msg_ualberta_sys_status.h | 65 +- .../mavlink/include/ualberta/testsuite.h | 121 +- .../mavlink/include/ualberta/ualberta.h | 4 + thirdParty/mavlink/include/ualberta/version.h | 4 + .../mavlink/message_definitions/common.xml | 3 +- 139 files changed, 12667 insertions(+), 4379 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_global_position.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_position_target.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 099981bf3..b00d3a018 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {8, 23, 12, 8, 14, 28, 3, 32, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 0, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 14, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 0, 6, 4, 0, 21, 18, 0, 0, 20, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {7, 33, 12, 0, 14, 28, 3, 32, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 101, 22, 26, 16, 14, 28, 28, 24, 0, 22, 22, 21, 0, 36, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 18, 16, 14, 14, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 3} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {153, 114, 143, 191, 105, 217, 104, 119, 0, 0, 0, 186, 194, 8, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 185, 222, 23, 179, 136, 66, 126, 198, 147, 0, 252, 162, 215, 229, 205, 51, 80, 101, 213, 8, 229, 21, 214, 170, 14, 73, 50, 142, 15, 3, 100, 24, 141, 148, 0, 0, 0, 183, 126, 130, 0, 148, 21, 0, 52, 124, 0, 0, 241, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 250, 156, 0, 0, 0, 0, 0, 0, 0, 146, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 15, 248, 63, 83, 127} +#define MAVLINK_MESSAGE_CRCS {88, 28, 137, 0, 237, 217, 104, 119, 0, 0, 0, 197, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 24, 23, 170, 144, 67, 115, 39, 231, 102, 0, 244, 237, 222, 0, 205, 51, 80, 101, 213, 8, 229, 21, 214, 41, 39, 131, 50, 142, 53, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 134, 219, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {NULL}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, {NULL}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, {NULL}, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {NULL}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE} #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h index e9cd44297..c0ce55a69 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -65,21 +65,41 @@ typedef struct __mavlink_sensor_offsets_t static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) { - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - - put_float_by_index(msg, 0, mag_declination); // magnetic declination (radians) - put_int32_t_by_index(msg, 4, raw_press); // raw pressure from barometer - put_int32_t_by_index(msg, 8, raw_temp); // raw temperature from barometer - put_float_by_index(msg, 12, gyro_cal_x); // gyro X calibration - put_float_by_index(msg, 16, gyro_cal_y); // gyro Y calibration - put_float_by_index(msg, 20, gyro_cal_z); // gyro Z calibration - put_float_by_index(msg, 24, accel_cal_x); // accel X calibration - put_float_by_index(msg, 28, accel_cal_y); // accel Y calibration - put_float_by_index(msg, 32, accel_cal_z); // accel Z calibration - put_int16_t_by_index(msg, 36, mag_ofs_x); // magnetometer X offset - put_int16_t_by_index(msg, 38, mag_ofs_y); // magnetometer Y offset - put_int16_t_by_index(msg, 40, mag_ofs_z); // magnetometer Z offset +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_sensor_offsets_t packet; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; return mavlink_finalize_message(msg, system_id, component_id, 42, 134); } @@ -107,21 +127,41 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u mavlink_message_t* msg, int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) { - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - - put_float_by_index(msg, 0, mag_declination); // magnetic declination (radians) - put_int32_t_by_index(msg, 4, raw_press); // raw pressure from barometer - put_int32_t_by_index(msg, 8, raw_temp); // raw temperature from barometer - put_float_by_index(msg, 12, gyro_cal_x); // gyro X calibration - put_float_by_index(msg, 16, gyro_cal_y); // gyro Y calibration - put_float_by_index(msg, 20, gyro_cal_z); // gyro Z calibration - put_float_by_index(msg, 24, accel_cal_x); // accel X calibration - put_float_by_index(msg, 28, accel_cal_y); // accel Y calibration - put_float_by_index(msg, 32, accel_cal_z); // accel Z calibration - put_int16_t_by_index(msg, 36, mag_ofs_x); // magnetometer X offset - put_int16_t_by_index(msg, 38, mag_ofs_y); // magnetometer Y offset - put_int16_t_by_index(msg, 40, mag_ofs_z); // magnetometer Z offset +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_sensor_offsets_t packet; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 134); } @@ -159,23 +199,39 @@ static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z) { - MAVLINK_ALIGNED_MESSAGE(msg, 42); - msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS; - - put_float_by_index(msg, 0, mag_declination); // magnetic declination (radians) - put_int32_t_by_index(msg, 4, raw_press); // raw pressure from barometer - put_int32_t_by_index(msg, 8, raw_temp); // raw temperature from barometer - put_float_by_index(msg, 12, gyro_cal_x); // gyro X calibration - put_float_by_index(msg, 16, gyro_cal_y); // gyro Y calibration - put_float_by_index(msg, 20, gyro_cal_z); // gyro Z calibration - put_float_by_index(msg, 24, accel_cal_x); // accel X calibration - put_float_by_index(msg, 28, accel_cal_y); // accel Y calibration - put_float_by_index(msg, 32, accel_cal_z); // accel Z calibration - put_int16_t_by_index(msg, 36, mag_ofs_x); // magnetometer X offset - put_int16_t_by_index(msg, 38, mag_ofs_y); // magnetometer Y offset - put_int16_t_by_index(msg, 40, mag_ofs_z); // magnetometer Z offset - - mavlink_finalize_message_chan_send(msg, chan, 42, 134); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_float(buf, 0, mag_declination); + _mav_put_int32_t(buf, 4, raw_press); + _mav_put_int32_t(buf, 8, raw_temp); + _mav_put_float(buf, 12, gyro_cal_x); + _mav_put_float(buf, 16, gyro_cal_y); + _mav_put_float(buf, 20, gyro_cal_z); + _mav_put_float(buf, 24, accel_cal_x); + _mav_put_float(buf, 28, accel_cal_y); + _mav_put_float(buf, 32, accel_cal_z); + _mav_put_int16_t(buf, 36, mag_ofs_x); + _mav_put_int16_t(buf, 38, mag_ofs_y); + _mav_put_int16_t(buf, 40, mag_ofs_z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, buf, 42, 134); +#else + mavlink_sensor_offsets_t packet; + packet.mag_declination = mag_declination; + packet.raw_press = raw_press; + packet.raw_temp = raw_temp; + packet.gyro_cal_x = gyro_cal_x; + packet.gyro_cal_y = gyro_cal_y; + packet.gyro_cal_z = gyro_cal_z; + packet.accel_cal_x = accel_cal_x; + packet.accel_cal_y = accel_cal_y; + packet.accel_cal_z = accel_cal_z; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_OFFSETS, (const char *)&packet, 42, 134); +#endif } #endif @@ -190,7 +246,7 @@ static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16 */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 36); + return _MAV_RETURN_int16_t(msg, 36); } /** @@ -200,7 +256,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_mes */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 38); + return _MAV_RETURN_int16_t(msg, 38); } /** @@ -210,7 +266,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_mes */ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 40); + return _MAV_RETURN_int16_t(msg, 40); } /** @@ -220,7 +276,7 @@ static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -230,7 +286,7 @@ static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink */ static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -240,7 +296,7 @@ static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_mes */ static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -250,7 +306,7 @@ static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -260,7 +316,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -270,7 +326,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -280,7 +336,7 @@ static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_mess */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -290,7 +346,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -300,7 +356,7 @@ static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_mes */ static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 32); + return _MAV_RETURN_float(msg, 32); } /** @@ -325,6 +381,6 @@ static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* ms sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg); sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg); #else - memcpy(sensor_offsets, MAVLINK_PAYLOAD(msg), 42); + memcpy(sensor_offsets, _MAV_PAYLOAD(msg), 42); #endif } diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h index 3083d7628..187cf243c 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -44,14 +44,27 @@ typedef struct __mavlink_set_mag_offsets_t static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) { - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - - put_int16_t_by_index(msg, 0, mag_ofs_x); // magnetometer X offset - put_int16_t_by_index(msg, 2, mag_ofs_y); // magnetometer Y offset - put_int16_t_by_index(msg, 4, mag_ofs_z); // magnetometer Z offset - put_uint8_t_by_index(msg, 6, target_system); // System ID - put_uint8_t_by_index(msg, 7, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_set_mag_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; return mavlink_finalize_message(msg, system_id, component_id, 8, 219); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) { - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - - put_int16_t_by_index(msg, 0, mag_ofs_x); // magnetometer X offset - put_int16_t_by_index(msg, 2, mag_ofs_y); // magnetometer Y offset - put_int16_t_by_index(msg, 4, mag_ofs_z); // magnetometer Z offset - put_uint8_t_by_index(msg, 6, target_system); // System ID - put_uint8_t_by_index(msg, 7, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_set_mag_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 219); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uin static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z) { - MAVLINK_ALIGNED_MESSAGE(msg, 8); - msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS; - - put_int16_t_by_index(msg, 0, mag_ofs_x); // magnetometer X offset - put_int16_t_by_index(msg, 2, mag_ofs_y); // magnetometer Y offset - put_int16_t_by_index(msg, 4, mag_ofs_z); // magnetometer Z offset - put_uint8_t_by_index(msg, 6, target_system); // System ID - put_uint8_t_by_index(msg, 7, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 8, 219); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_int16_t(buf, 0, mag_ofs_x); + _mav_put_int16_t(buf, 2, mag_ofs_y); + _mav_put_int16_t(buf, 4, mag_ofs_z); + _mav_put_uint8_t(buf, 6, target_system); + _mav_put_uint8_t(buf, 7, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, buf, 8, 219); +#else + mavlink_set_mag_offsets_t packet; + packet.mag_ofs_x = mag_ofs_x; + packet.mag_ofs_y = mag_ofs_y; + packet.mag_ofs_z = mag_ofs_z; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MAG_OFFSETS, (const char *)&packet, 8, 219); +#endif } #endif @@ -134,7 +169,7 @@ static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint */ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -144,7 +179,7 @@ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlin */ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 7); + return _MAV_RETURN_uint8_t(msg, 7); } /** @@ -154,7 +189,7 @@ static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mav */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 0); + return _MAV_RETURN_int16_t(msg, 0); } /** @@ -164,7 +199,7 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_me */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 2); + return _MAV_RETURN_int16_t(msg, 2); } /** @@ -174,7 +209,7 @@ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_me */ static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 4); + return _MAV_RETURN_int16_t(msg, 4); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* m set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg); set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg); #else - memcpy(set_mag_offsets, MAVLINK_PAYLOAD(msg), 8); + memcpy(set_mag_offsets, _MAV_PAYLOAD(msg), 8); #endif } diff --git a/thirdParty/mavlink/include/ardupilotmega/testsuite.h b/thirdParty/mavlink/include/ardupilotmega/testsuite.h index 2fd63d85c..fb4ca0a30 100644 --- a/thirdParty/mavlink/include/ardupilotmega/testsuite.h +++ b/thirdParty/mavlink/include/ardupilotmega/testsuite.h @@ -11,25 +11,25 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t); -static void mavlink_test_ardupilotmega(uint8_t, uint8_t); +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_ardupilotmega(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_common(system_id, component_id); - mavlink_test_ardupilotmega(system_id, component_id); + mavlink_test_common(system_id, component_id, last_msg); + mavlink_test_ardupilotmega(system_id, component_id, last_msg); } #endif #include "../common/testsuite.h" -static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id) +static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_sensor_offsets_t packet2, packet1 = { + mavlink_sensor_offsets_t packet_in = { 17.0, 963497672, 963497880, @@ -43,52 +43,107 @@ static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id) 19211, 19315, }; - if (sizeof(packet2) != 42) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_sensor_offsets_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mag_declination = packet_in.mag_declination; + packet1.raw_press = packet_in.raw_press; + packet1.raw_temp = packet_in.raw_temp; + packet1.gyro_cal_x = packet_in.gyro_cal_x; + packet1.gyro_cal_y = packet_in.gyro_cal_y; + packet1.gyro_cal_z = packet_in.gyro_cal_z; + packet1.accel_cal_x = packet_in.accel_cal_x; + packet1.accel_cal_y = packet_in.accel_cal_y; + packet1.accel_cal_z = packet_in.accel_cal_z; + packet1.mag_ofs_x = packet_in.mag_ofs_x; + packet1.mag_ofs_y = packet_in.mag_ofs_y; + packet1.mag_ofs_z = packet_in.mag_ofs_z; + + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_sensor_offsets_encode(system_id, component_id, &msg, &packet1); mavlink_msg_sensor_offsets_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_sensor_offsets_pack(system_id, component_id, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_sensor_offsets_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.mag_ofs_x , packet1.mag_ofs_y , packet1.mag_ofs_z , packet1.mag_declination , packet1.raw_press , packet1.raw_temp , packet1.gyro_cal_x , packet1.gyro_cal_y , packet1.gyro_cal_z , packet1.accel_cal_x , packet1.accel_cal_y , packet1.accel_cal_z ); + mavlink_msg_sensor_offsets_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; imsgid = MAVLINK_MSG_ID_ATTITUDE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); + + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, roll); // Roll angle (rad) - put_float_by_index(msg, 8, pitch); // Pitch angle (rad) - put_float_by_index(msg, 12, yaw); // Yaw angle (rad) - put_float_by_index(msg, 16, rollspeed); // Roll angular speed (rad/s) - put_float_by_index(msg, 20, pitchspeed); // Pitch angular speed (rad/s) - put_float_by_index(msg, 24, yawspeed); // Yaw angular speed (rad/s) + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; return mavlink_finalize_message(msg, system_id, component_id, 28, 39); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, uint32_t time_boot_ms,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed) { - msg->msgid = MAVLINK_MSG_ID_ATTITUDE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, roll); // Roll angle (rad) - put_float_by_index(msg, 8, pitch); // Pitch angle (rad) - put_float_by_index(msg, 12, yaw); // Yaw angle (rad) - put_float_by_index(msg, 16, rollspeed); // Roll angular speed (rad/s) - put_float_by_index(msg, 20, pitchspeed); // Pitch angular speed (rad/s) - put_float_by_index(msg, 24, yawspeed); // Yaw angular speed (rad/s) + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 39); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { - MAVLINK_ALIGNED_MESSAGE(msg, 28); - msg->msgid = MAVLINK_MSG_ID_ATTITUDE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, rollspeed); + _mav_put_float(buf, 20, pitchspeed); + _mav_put_float(buf, 24, yawspeed); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, roll); // Roll angle (rad) - put_float_by_index(msg, 8, pitch); // Pitch angle (rad) - put_float_by_index(msg, 12, yaw); // Yaw angle (rad) - put_float_by_index(msg, 16, rollspeed); // Roll angular speed (rad/s) - put_float_by_index(msg, 20, pitchspeed); // Pitch angular speed (rad/s) - put_float_by_index(msg, 24, yawspeed); // Yaw angular speed (rad/s) + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, buf, 28, 39); +#else + mavlink_attitude_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; - mavlink_finalize_message_chan_send(msg, chan, 28, 39); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE, (const char *)&packet, 28, 39); +#endif } #endif @@ -150,7 +191,7 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint32_t ti */ static inline uint32_t mavlink_msg_attitude_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -160,7 +201,7 @@ static inline uint32_t mavlink_msg_attitude_get_time_boot_ms(const mavlink_messa */ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -170,7 +211,7 @@ static inline float mavlink_msg_attitude_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -180,7 +221,7 @@ static inline float mavlink_msg_attitude_get_pitch(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -190,7 +231,7 @@ static inline float mavlink_msg_attitude_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -200,7 +241,7 @@ static inline float mavlink_msg_attitude_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -210,7 +251,7 @@ static inline float mavlink_msg_attitude_get_pitchspeed(const mavlink_message_t* */ static inline float mavlink_msg_attitude_get_yawspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_attitude_decode(const mavlink_message_t* msg, mav attitude->pitchspeed = mavlink_msg_attitude_get_pitchspeed(msg); attitude->yawspeed = mavlink_msg_attitude_get_yawspeed(msg); #else - memcpy(attitude, MAVLINK_PAYLOAD(msg), 28); + memcpy(attitude, _MAV_PAYLOAD(msg), 28); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index ad7e8c461..5efb8d6b9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -32,10 +32,19 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_auth_key_t packet; - put_char_array_by_index(msg, 0, key, 32); // key + memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; return mavlink_finalize_message(msg, system_id, component_id, 32, 119); } @@ -52,10 +61,19 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, const char *key) { - msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; - put_char_array_by_index(msg, 0, key, 32); // key + _mav_put_char_array(buf, 0, key, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_auth_key_t packet; + memcpy(packet.key, key, sizeof(char)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 119); } @@ -82,12 +100,17 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char *key) { - MAVLINK_ALIGNED_MESSAGE(msg, 32); - msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; - put_char_array_by_index(msg, 0, key, 32); // key + _mav_put_char_array(buf, 0, key, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, buf, 32, 119); +#else + mavlink_auth_key_t packet; - mavlink_finalize_message_chan_send(msg, chan, 32, 119); + memcpy(packet.key, key, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTH_KEY, (const char *)&packet, 32, 119); +#endif } #endif @@ -102,7 +125,7 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char */ static inline uint16_t mavlink_msg_auth_key_get_key(const mavlink_message_t* msg, char *key) { - return MAVLINK_MSG_RETURN_char_array(msg, key, 32, 0); + return _MAV_RETURN_char_array(msg, key, 32, 0); } /** @@ -116,6 +139,6 @@ static inline void mavlink_msg_auth_key_decode(const mavlink_message_t* msg, mav #if MAVLINK_NEED_BYTE_SWAP mavlink_msg_auth_key_get_key(msg, auth_key->key); #else - memcpy(auth_key, MAVLINK_PAYLOAD(msg), 32); + memcpy(auth_key, _MAV_PAYLOAD(msg), 32); #endif } 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 29193f472..0dc34eb0d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -41,13 +41,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - - put_uint8_t_by_index(msg, 0, target_system); // System the GCS requests control for - put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV - put_uint8_t_by_index(msg, 2, version); // 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - put_char_array_by_index(msg, 3, 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 "!?,.-" +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; return mavlink_finalize_message(msg, system_id, component_id, 28, 217); } @@ -67,13 +77,23 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys mavlink_message_t* msg, uint8_t target_system,uint8_t control_request,uint8_t version,const char *passkey) { - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - - put_uint8_t_by_index(msg, 0, target_system); // System the GCS requests control for - put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV - put_uint8_t_by_index(msg, 2, version); // 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - put_char_array_by_index(msg, 3, 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 "!?,.-" +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + memcpy(packet.passkey, passkey, sizeof(char)*25); + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 217); } @@ -103,15 +123,21 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system 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_ALIGNED_MESSAGE(msg, 28); - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - - put_uint8_t_by_index(msg, 0, target_system); // System the GCS requests control for - put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV - put_uint8_t_by_index(msg, 2, version); // 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - put_char_array_by_index(msg, 3, 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_finalize_message_chan_send(msg, chan, 28, 217); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, version); + _mav_put_char_array(buf, 3, passkey, 25); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, buf, 28, 217); +#else + mavlink_change_operator_control_t packet; + packet.target_system = target_system; + packet.control_request = control_request; + packet.version = version; + memcpy(packet.passkey, passkey, sizeof(char)*25); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, (const char *)&packet, 28, 217); +#endif } #endif @@ -126,7 +152,7 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -136,7 +162,7 @@ static inline uint8_t mavlink_msg_change_operator_control_get_target_system(cons */ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -146,7 +172,7 @@ static inline uint8_t mavlink_msg_change_operator_control_get_control_request(co */ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -156,7 +182,7 @@ static inline uint8_t mavlink_msg_change_operator_control_get_version(const mavl */ static inline uint16_t mavlink_msg_change_operator_control_get_passkey(const mavlink_message_t* msg, char *passkey) { - return MAVLINK_MSG_RETURN_char_array(msg, passkey, 25, 3); + return _MAV_RETURN_char_array(msg, passkey, 25, 3); } /** @@ -173,6 +199,6 @@ static inline void mavlink_msg_change_operator_control_decode(const mavlink_mess change_operator_control->version = mavlink_msg_change_operator_control_get_version(msg); mavlink_msg_change_operator_control_get_passkey(msg, change_operator_control->passkey); #else - memcpy(change_operator_control, MAVLINK_PAYLOAD(msg), 28); + memcpy(change_operator_control, _MAV_PAYLOAD(msg), 28); #endif } 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 674820dbb..d5b76c7ca 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 @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; - put_uint8_t_by_index(msg, 0, gcs_system_id); // ID of the GCS this message - put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV - put_uint8_t_by_index(msg, 2, ack); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; return mavlink_finalize_message(msg, system_id, component_id, 3, 104); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t mavlink_message_t* msg, uint8_t gcs_system_id,uint8_t control_request,uint8_t ack) { - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); - put_uint8_t_by_index(msg, 0, gcs_system_id); // ID of the GCS this message - put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV - put_uint8_t_by_index(msg, 2, ack); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { - MAVLINK_ALIGNED_MESSAGE(msg, 3); - msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, gcs_system_id); + _mav_put_uint8_t(buf, 1, control_request); + _mav_put_uint8_t(buf, 2, ack); - put_uint8_t_by_index(msg, 0, gcs_system_id); // ID of the GCS this message - put_uint8_t_by_index(msg, 1, control_request); // 0: request control of this MAV, 1: Release control of this MAV - put_uint8_t_by_index(msg, 2, ack); // 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, buf, 3, 104); +#else + mavlink_change_operator_control_ack_t packet; + packet.gcs_system_id = gcs_system_id; + packet.control_request = control_request; + packet.ack = ack; - mavlink_finalize_message_chan_send(msg, chan, 3, 104); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, (const char *)&packet, 3, 104); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_gcs_system_id( */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_request(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_change_operator_control_ack_get_control_reques */ static inline uint8_t mavlink_msg_change_operator_control_ack_get_ack(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_change_operator_control_ack_decode(const mavlink_ change_operator_control_ack->control_request = mavlink_msg_change_operator_control_ack_get_control_request(msg); change_operator_control_ack->ack = mavlink_msg_change_operator_control_ack_get_ack(msg); #else - memcpy(change_operator_control_ack, MAVLINK_PAYLOAD(msg), 3); + memcpy(change_operator_control_ack, _MAV_PAYLOAD(msg), 3); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index d74520446..8fc347e81 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -35,11 +35,21 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_float(buf, 0, command); + _mav_put_float(buf, 4, result); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; - put_float_by_index(msg, 0, command); // Current airspeed in m/s - put_float_by_index(msg, 4, result); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; return mavlink_finalize_message(msg, system_id, component_id, 8, 8); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, float command,float result) { - msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_float(buf, 0, command); + _mav_put_float(buf, 4, result); - put_float_by_index(msg, 0, command); // Current airspeed in m/s - put_float_by_index(msg, 4, result); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 8); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { - MAVLINK_ALIGNED_MESSAGE(msg, 8); - msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_float(buf, 0, command); + _mav_put_float(buf, 4, result); - put_float_by_index(msg, 0, command); // Current airspeed in m/s - put_float_by_index(msg, 4, result); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, 8, 8); +#else + mavlink_command_ack_t packet; + packet.command = command; + packet.result = result; - mavlink_finalize_message_chan_send(msg, chan, 8, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, 8, 8); +#endif } #endif @@ -110,7 +136,7 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float co */ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -120,7 +146,7 @@ static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* */ static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, command_ack->command = mavlink_msg_command_ack_get_command(msg); command_ack->result = mavlink_msg_command_ack_get_result(msg); #else - memcpy(command_ack, MAVLINK_PAYLOAD(msg), 8); + memcpy(command_ack, _MAV_PAYLOAD(msg), 8); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_long.h b/thirdParty/mavlink/include/common/mavlink_msg_command_long.h index 43a05eaa3..2c8e0af44 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_long.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_long.h @@ -62,20 +62,39 @@ typedef struct __mavlink_command_long_t static inline uint16_t mavlink_msg_command_long_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, float param5, float param6, float param7) { - msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - - put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. - put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. - put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. - put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. - put_float_by_index(msg, 16, param5); // Parameter 5, as defined by MAV_CMD enum. - put_float_by_index(msg, 20, param6); // Parameter 6, as defined by MAV_CMD enum. - put_float_by_index(msg, 24, param7); // Parameter 7, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 28, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 29, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 30, command); // Command ID, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 31, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 30, command); + _mav_put_uint8_t(buf, 31, confirmation); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; return mavlink_finalize_message(msg, system_id, component_id, 32, 168); } @@ -102,20 +121,39 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin 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,float param5,float param6,float param7) { - msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - - put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. - put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. - put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. - put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. - put_float_by_index(msg, 16, param5); // Parameter 5, as defined by MAV_CMD enum. - put_float_by_index(msg, 20, param6); // Parameter 6, as defined by MAV_CMD enum. - put_float_by_index(msg, 24, param7); // Parameter 7, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 28, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 29, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 30, command); // Command ID, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 31, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 30, command); + _mav_put_uint8_t(buf, 31, confirmation); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 168); } @@ -152,22 +190,37 @@ static inline uint16_t mavlink_msg_command_long_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_command_long_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, float param5, float param6, float param7) { - MAVLINK_ALIGNED_MESSAGE(msg, 32); - msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG; - - put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. - put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. - put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. - put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. - put_float_by_index(msg, 16, param5); // Parameter 5, as defined by MAV_CMD enum. - put_float_by_index(msg, 20, param6); // Parameter 6, as defined by MAV_CMD enum. - put_float_by_index(msg, 24, param7); // Parameter 7, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 28, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 29, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 30, command); // Command ID, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 31, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - - mavlink_finalize_message_chan_send(msg, chan, 32, 168); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, param5); + _mav_put_float(buf, 20, param6); + _mav_put_float(buf, 24, param7); + _mav_put_uint8_t(buf, 28, target_system); + _mav_put_uint8_t(buf, 29, target_component); + _mav_put_uint8_t(buf, 30, command); + _mav_put_uint8_t(buf, 31, confirmation); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, buf, 32, 168); +#else + mavlink_command_long_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.param5 = param5; + packet.param6 = param6; + packet.param7 = param7; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_LONG, (const char *)&packet, 32, 168); +#endif } #endif @@ -182,7 +235,7 @@ static inline void mavlink_msg_command_long_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 28); + return _MAV_RETURN_uint8_t(msg, 28); } /** @@ -192,7 +245,7 @@ static inline uint8_t mavlink_msg_command_long_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 29); + return _MAV_RETURN_uint8_t(msg, 29); } /** @@ -202,7 +255,7 @@ static inline uint8_t mavlink_msg_command_long_get_target_component(const mavlin */ static inline uint8_t mavlink_msg_command_long_get_command(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 30); + return _MAV_RETURN_uint8_t(msg, 30); } /** @@ -212,7 +265,7 @@ static inline uint8_t mavlink_msg_command_long_get_command(const mavlink_message */ static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 31); + return _MAV_RETURN_uint8_t(msg, 31); } /** @@ -222,7 +275,7 @@ static inline uint8_t mavlink_msg_command_long_get_confirmation(const mavlink_me */ static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -232,7 +285,7 @@ static inline float mavlink_msg_command_long_get_param1(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -242,7 +295,7 @@ static inline float mavlink_msg_command_long_get_param2(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -252,7 +305,7 @@ static inline float mavlink_msg_command_long_get_param3(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -262,7 +315,7 @@ static inline float mavlink_msg_command_long_get_param4(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -272,7 +325,7 @@ static inline float mavlink_msg_command_long_get_param5(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -282,7 +335,7 @@ static inline float mavlink_msg_command_long_get_param6(const mavlink_message_t* */ static inline float mavlink_msg_command_long_get_param7(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -306,6 +359,6 @@ static inline void mavlink_msg_command_long_decode(const mavlink_message_t* msg, command_long->command = mavlink_msg_command_long_get_command(msg); command_long->confirmation = mavlink_msg_command_long_get_confirmation(msg); #else - memcpy(command_long, MAVLINK_PAYLOAD(msg), 32); + memcpy(command_long, _MAV_PAYLOAD(msg), 32); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_short.h b/thirdParty/mavlink/include/common/mavlink_msg_command_short.h index 7a9a6e62d..35919cb2a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_short.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_short.h @@ -53,17 +53,33 @@ typedef struct __mavlink_command_short_t static inline uint16_t mavlink_msg_command_short_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) { - msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint8_t(buf, 18, command); + _mav_put_uint8_t(buf, 19, confirmation); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_command_short_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; - put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. - put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. - put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. - put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 16, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 17, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 18, command); // Command ID, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 19, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; return mavlink_finalize_message(msg, system_id, component_id, 20, 160); } @@ -87,17 +103,33 @@ static inline uint16_t mavlink_msg_command_short_pack_chan(uint8_t system_id, ui 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) { - msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint8_t(buf, 18, command); + _mav_put_uint8_t(buf, 19, confirmation); - put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. - put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. - put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. - put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 16, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 17, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 18, command); // Command ID, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 19, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_command_short_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 160); } @@ -131,19 +163,31 @@ static inline uint16_t mavlink_msg_command_short_encode(uint8_t system_id, uint8 static inline void mavlink_msg_command_short_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_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint8_t(buf, 18, command); + _mav_put_uint8_t(buf, 19, confirmation); - put_float_by_index(msg, 0, param1); // Parameter 1, as defined by MAV_CMD enum. - put_float_by_index(msg, 4, param2); // Parameter 2, as defined by MAV_CMD enum. - put_float_by_index(msg, 8, param3); // Parameter 3, as defined by MAV_CMD enum. - put_float_by_index(msg, 12, param4); // Parameter 4, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 16, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 17, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 18, command); // Command ID, as defined by MAV_CMD enum. - put_uint8_t_by_index(msg, 19, confirmation); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_SHORT, buf, 20, 160); +#else + mavlink_command_short_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.target_system = target_system; + packet.target_component = target_component; + packet.command = command; + packet.confirmation = confirmation; - mavlink_finalize_message_chan_send(msg, chan, 20, 160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_SHORT, (const char *)&packet, 20, 160); +#endif } #endif @@ -158,7 +202,7 @@ static inline void mavlink_msg_command_short_send(mavlink_channel_t chan, uint8_ */ static inline uint8_t mavlink_msg_command_short_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -168,7 +212,7 @@ static inline uint8_t mavlink_msg_command_short_get_target_system(const mavlink_ */ static inline uint8_t mavlink_msg_command_short_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -178,7 +222,7 @@ static inline uint8_t mavlink_msg_command_short_get_target_component(const mavli */ static inline uint8_t mavlink_msg_command_short_get_command(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 18); + return _MAV_RETURN_uint8_t(msg, 18); } /** @@ -188,7 +232,7 @@ static inline uint8_t mavlink_msg_command_short_get_command(const mavlink_messag */ static inline uint8_t mavlink_msg_command_short_get_confirmation(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 19); + return _MAV_RETURN_uint8_t(msg, 19); } /** @@ -198,7 +242,7 @@ static inline uint8_t mavlink_msg_command_short_get_confirmation(const mavlink_m */ static inline float mavlink_msg_command_short_get_param1(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -208,7 +252,7 @@ static inline float mavlink_msg_command_short_get_param1(const mavlink_message_t */ static inline float mavlink_msg_command_short_get_param2(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -218,7 +262,7 @@ static inline float mavlink_msg_command_short_get_param2(const mavlink_message_t */ static inline float mavlink_msg_command_short_get_param3(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -228,7 +272,7 @@ static inline float mavlink_msg_command_short_get_param3(const mavlink_message_t */ static inline float mavlink_msg_command_short_get_param4(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -249,6 +293,6 @@ static inline void mavlink_msg_command_short_decode(const mavlink_message_t* msg command_short->command = mavlink_msg_command_short_get_command(msg); command_short->confirmation = mavlink_msg_command_short_get_confirmation(msg); #else - memcpy(command_short, MAVLINK_PAYLOAD(msg), 20); + memcpy(command_short, _MAV_PAYLOAD(msg), 20); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h index efcb7c9b8..94e2874a0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h @@ -38,12 +38,23 @@ typedef struct __mavlink_data_stream_t static inline uint16_t mavlink_msg_data_stream_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) { - msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; - put_uint16_t_by_index(msg, 0, message_rate); // The requested interval between two messages of this type - put_uint8_t_by_index(msg, 2, stream_id); // The ID of the requested data stream - put_uint8_t_by_index(msg, 3, on_off); // 1 stream is enabled, 0 stream is stopped. + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; return mavlink_finalize_message(msg, system_id, component_id, 4, 21); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, uint8_t stream_id,uint16_t message_rate,uint8_t on_off) { - msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); - put_uint16_t_by_index(msg, 0, message_rate); // The requested interval between two messages of this type - put_uint8_t_by_index(msg, 2, stream_id); // The ID of the requested data stream - put_uint8_t_by_index(msg, 3, on_off); // 1 stream is enabled, 0 stream is stopped. + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 21); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_data_stream_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t message_rate, uint8_t on_off) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, message_rate); + _mav_put_uint8_t(buf, 2, stream_id); + _mav_put_uint8_t(buf, 3, on_off); - put_uint16_t_by_index(msg, 0, message_rate); // The requested interval between two messages of this type - put_uint8_t_by_index(msg, 2, stream_id); // The ID of the requested data stream - put_uint8_t_by_index(msg, 3, on_off); // 1 stream is enabled, 0 stream is stopped. + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, buf, 4, 21); +#else + mavlink_data_stream_t packet; + packet.message_rate = message_rate; + packet.stream_id = stream_id; + packet.on_off = on_off; - mavlink_finalize_message_chan_send(msg, chan, 4, 21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_STREAM, (const char *)&packet, 4, 21); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_data_stream_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_data_stream_get_stream_id(const mavlink_messag */ static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -138,7 +167,7 @@ static inline uint16_t mavlink_msg_data_stream_get_message_rate(const mavlink_me */ static inline uint8_t mavlink_msg_data_stream_get_on_off(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_data_stream_decode(const mavlink_message_t* msg, data_stream->stream_id = mavlink_msg_data_stream_get_stream_id(msg); data_stream->on_off = mavlink_msg_data_stream_get_on_off(msg); #else - memcpy(data_stream, MAVLINK_PAYLOAD(msg), 4); + memcpy(data_stream, _MAV_PAYLOAD(msg), 4); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index ef67055d1..e85cf5804 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -38,12 +38,23 @@ 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, uint32_t time_boot_ms, uint8_t ind, float value) { - msg->msgid = MAVLINK_MSG_ID_DEBUG; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[9]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); + + memcpy(_MAV_PAYLOAD(msg), buf, 9); +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, value); // DEBUG value - put_uint8_t_by_index(msg, 8, ind); // index of debug variable + memcpy(_MAV_PAYLOAD(msg), &packet, 9); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG; return mavlink_finalize_message(msg, system_id, component_id, 9, 46); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co mavlink_message_t* msg, uint32_t time_boot_ms,uint8_t ind,float value) { - msg->msgid = MAVLINK_MSG_ID_DEBUG; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[9]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, value); // DEBUG value - put_uint8_t_by_index(msg, 8, ind); // index of debug variable + memcpy(_MAV_PAYLOAD(msg), buf, 9); +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; + memcpy(_MAV_PAYLOAD(msg), &packet, 9); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 9, 46); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t ind, float value) { - MAVLINK_ALIGNED_MESSAGE(msg, 9); - msg->msgid = MAVLINK_MSG_ID_DEBUG; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[9]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_uint8_t(buf, 8, ind); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, value); // DEBUG value - put_uint8_t_by_index(msg, 8, ind); // index of debug variable + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, buf, 9, 46); +#else + mavlink_debug_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + packet.ind = ind; - mavlink_finalize_message_chan_send(msg, chan, 9, 46); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG, (const char *)&packet, 9, 46); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint32_t time_ */ static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -128,7 +157,7 @@ static inline uint32_t mavlink_msg_debug_get_time_boot_ms(const mavlink_message_ */ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 8); + return _MAV_RETURN_uint8_t(msg, 8); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_debug_get_ind(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_get_value(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_debug_decode(const mavlink_message_t* msg, mavlin debug->value = mavlink_msg_debug_get_value(msg); debug->ind = mavlink_msg_debug_get_ind(msg); #else - memcpy(debug, MAVLINK_PAYLOAD(msg), 9); + memcpy(debug, _MAV_PAYLOAD(msg), 9); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index c614c4378..e92eb479f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -44,14 +44,25 @@ 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 time_usec, float x, float y, float z) { - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp - put_float_by_index(msg, 8, x); // x - put_float_by_index(msg, 12, y); // y - put_float_by_index(msg, 16, z); // z - put_char_array_by_index(msg, 20, name, 10); // Name +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); + memcpy(_MAV_PAYLOAD(msg), buf, 30); +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; return mavlink_finalize_message(msg, system_id, component_id, 30, 49); } @@ -72,14 +83,25 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 mavlink_message_t* msg, const char *name,uint64_t time_usec,float x,float y,float z) { - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp - put_float_by_index(msg, 8, x); // x - put_float_by_index(msg, 12, y); // y - put_float_by_index(msg, 16, z); // z - put_char_array_by_index(msg, 20, name, 10); // Name +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); + memcpy(_MAV_PAYLOAD(msg), buf, 30); +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 49); } @@ -110,16 +132,23 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char *name, uint64_t time_usec, float x, float y, float z) { - MAVLINK_ALIGNED_MESSAGE(msg, 30); - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp - put_float_by_index(msg, 8, x); // x - put_float_by_index(msg, 12, y); // y - put_float_by_index(msg, 16, z); // z - put_char_array_by_index(msg, 20, name, 10); // Name - - mavlink_finalize_message_chan_send(msg, chan, 30, 49); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_char_array(buf, 20, name, 10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, buf, 30, 49); +#else + mavlink_debug_vect_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + memcpy(packet.name, name, sizeof(char)*10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_VECT, (const char *)&packet, 30, 49); +#endif } #endif @@ -134,7 +163,7 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha */ static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* msg, char *name) { - return MAVLINK_MSG_RETURN_char_array(msg, name, 10, 20); + return _MAV_RETURN_char_array(msg, name, 10, 20); } /** @@ -144,7 +173,7 @@ static inline uint16_t mavlink_msg_debug_vect_get_name(const mavlink_message_t* */ static inline uint64_t mavlink_msg_debug_vect_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -154,7 +183,7 @@ static inline uint64_t mavlink_msg_debug_vect_get_time_usec(const mavlink_messag */ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -164,7 +193,7 @@ static inline float mavlink_msg_debug_vect_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -174,7 +203,7 @@ static inline float mavlink_msg_debug_vect_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_debug_vect_get_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -192,6 +221,6 @@ static inline void mavlink_msg_debug_vect_decode(const mavlink_message_t* msg, m debug_vect->z = mavlink_msg_debug_vect_get_z(msg); mavlink_msg_debug_vect_get_name(msg, debug_vect->name); #else - memcpy(debug_vect, MAVLINK_PAYLOAD(msg), 30); + memcpy(debug_vect, _MAV_PAYLOAD(msg), 30); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_extended_message.h b/thirdParty/mavlink/include/common/mavlink_msg_extended_message.h index 3c8f65ab3..190cf5d88 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_extended_message.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_extended_message.h @@ -38,12 +38,23 @@ typedef struct __mavlink_extended_message_t static inline uint16_t mavlink_msg_extended_message_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t protocol_flags) { - msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, protocol_flags); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_extended_message_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.protocol_flags = protocol_flags; - put_uint8_t_by_index(msg, 0, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 1, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 2, protocol_flags); // Retransmission / ACK flags + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; return mavlink_finalize_message(msg, system_id, component_id, 3, 247); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_extended_message_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint8_t protocol_flags) { - msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, protocol_flags); - put_uint8_t_by_index(msg, 0, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 1, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 2, protocol_flags); // Retransmission / ACK flags + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_extended_message_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.protocol_flags = protocol_flags; + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 247); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_extended_message_encode(uint8_t system_id, ui static inline void mavlink_msg_extended_message_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t protocol_flags) { - MAVLINK_ALIGNED_MESSAGE(msg, 3); - msg->msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, protocol_flags); - put_uint8_t_by_index(msg, 0, target_system); // System which should execute the command - put_uint8_t_by_index(msg, 1, target_component); // Component which should execute the command, 0 for all components - put_uint8_t_by_index(msg, 2, protocol_flags); // Retransmission / ACK flags + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTENDED_MESSAGE, buf, 3, 247); +#else + mavlink_extended_message_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.protocol_flags = protocol_flags; - mavlink_finalize_message_chan_send(msg, chan, 3, 247); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EXTENDED_MESSAGE, (const char *)&packet, 3, 247); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_extended_message_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_extended_message_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_extended_message_get_target_system(const mavli */ static inline uint8_t mavlink_msg_extended_message_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_extended_message_get_target_component(const ma */ static inline uint8_t mavlink_msg_extended_message_get_protocol_flags(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_extended_message_decode(const mavlink_message_t* extended_message->target_component = mavlink_msg_extended_message_get_target_component(msg); extended_message->protocol_flags = mavlink_msg_extended_message_get_protocol_flags(msg); #else - memcpy(extended_message, MAVLINK_PAYLOAD(msg), 3); + memcpy(extended_message, _MAV_PAYLOAD(msg), 3); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h new file mode 100644 index 000000000..db1943b83 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -0,0 +1,276 @@ +// MESSAGE GLOBAL_POSITION PACKING + +#define MAVLINK_MSG_ID_GLOBAL_POSITION 33 + +typedef struct __mavlink_global_position_t +{ + uint64_t usec; ///< Timestamp (microseconds since unix epoch) + float lat; ///< Latitude, in degrees + float lon; ///< Longitude, in degrees + float alt; ///< Absolute altitude, in meters + float vx; ///< X Speed (in Latitude direction, positive: going north) + float vy; ///< Y Speed (in Longitude direction, positive: going east) + float vz; ///< Z Speed (in Altitude direction, positive: going up) +} mavlink_global_position_t; + +#define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 +#define MAVLINK_MSG_ID_33_LEN 32 + + + +#define MAVLINK_MESSAGE_INFO_GLOBAL_POSITION { \ + "GLOBAL_POSITION", \ + 7, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_position_t, usec) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_position_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_position_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_global_position_t, alt) }, \ + { "vx", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_position_t, vx) }, \ + { "vy", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_position_t, vy) }, \ + { "vz", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_position_t, vz) }, \ + } \ +} + + +/** + * @brief Pack a global_position message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since unix epoch) + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_global_position_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + return mavlink_finalize_message(msg, system_id, component_id, 32, 147); +} + +/** + * @brief Pack a global_position message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since unix epoch) + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,float lat,float lon,float alt,float vx,float vy,float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_global_position_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 147); +} + +/** + * @brief Encode a global_position struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param global_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_position_t* global_position) +{ + return mavlink_msg_global_position_pack(system_id, component_id, msg, global_position->usec, global_position->lat, global_position->lon, global_position->alt, global_position->vx, global_position->vy, global_position->vz); +} + +/** + * @brief Send a global_position message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since unix epoch) + * @param lat Latitude, in degrees + * @param lon Longitude, in degrees + * @param alt Absolute altitude, in meters + * @param vx X Speed (in Latitude direction, positive: going north) + * @param vy Y Speed (in Longitude direction, positive: going east) + * @param vz Z Speed (in Altitude direction, positive: going up) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, vx); + _mav_put_float(buf, 24, vy); + _mav_put_float(buf, 28, vz); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION, buf, 32, 147); +#else + mavlink_global_position_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION, (const char *)&packet, 32, 147); +#endif +} + +#endif + +// MESSAGE GLOBAL_POSITION UNPACKING + + +/** + * @brief Get field usec from global_position message + * + * @return Timestamp (microseconds since unix epoch) + */ +static inline uint64_t mavlink_msg_global_position_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field lat from global_position message + * + * @return Latitude, in degrees + */ +static inline float mavlink_msg_global_position_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field lon from global_position message + * + * @return Longitude, in degrees + */ +static inline float mavlink_msg_global_position_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field alt from global_position message + * + * @return Absolute altitude, in meters + */ +static inline float mavlink_msg_global_position_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vx from global_position message + * + * @return X Speed (in Latitude direction, positive: going north) + */ +static inline float mavlink_msg_global_position_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vy from global_position message + * + * @return Y Speed (in Longitude direction, positive: going east) + */ +static inline float mavlink_msg_global_position_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field vz from global_position message + * + * @return Z Speed (in Altitude direction, positive: going up) + */ +static inline float mavlink_msg_global_position_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a global_position message into a struct + * + * @param msg The message to decode + * @param global_position C-struct to decode the message contents into + */ +static inline void mavlink_msg_global_position_decode(const mavlink_message_t* msg, mavlink_global_position_t* global_position) +{ +#if MAVLINK_NEED_BYTE_SWAP + global_position->usec = mavlink_msg_global_position_get_usec(msg); + global_position->lat = mavlink_msg_global_position_get_lat(msg); + global_position->lon = mavlink_msg_global_position_get_lon(msg); + global_position->alt = mavlink_msg_global_position_get_alt(msg); + global_position->vx = mavlink_msg_global_position_get_vx(msg); + global_position->vy = mavlink_msg_global_position_get_vy(msg); + global_position->vz = mavlink_msg_global_position_get_vz(msg); +#else + memcpy(global_position, _MAV_PAYLOAD(msg), 32); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h index f70083ab9..3177c0da6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -53,17 +53,33 @@ 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, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int16_t(buf, 16, vx); + _mav_put_int16_t(buf, 18, vy); + _mav_put_int16_t(buf, 20, vz); + _mav_put_uint16_t(buf, 22, hdg); + + memcpy(_MAV_PAYLOAD(msg), buf, 24); +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int32_t_by_index(msg, 4, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 8, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 12, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL - put_int16_t_by_index(msg, 16, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 18, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 20, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_uint16_t_by_index(msg, 22, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + memcpy(_MAV_PAYLOAD(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; return mavlink_finalize_message(msg, system_id, component_id, 24, 102); } @@ -87,17 +103,33 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ mavlink_message_t* msg, uint32_t time_boot_ms,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,uint16_t hdg) { - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int16_t(buf, 16, vx); + _mav_put_int16_t(buf, 18, vy); + _mav_put_int16_t(buf, 20, vz); + _mav_put_uint16_t(buf, 22, hdg); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int32_t_by_index(msg, 4, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 8, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 12, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL - put_int16_t_by_index(msg, 16, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 18, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 20, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_uint16_t_by_index(msg, 22, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + memcpy(_MAV_PAYLOAD(msg), buf, 24); +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; + memcpy(_MAV_PAYLOAD(msg), &packet, 24); +#endif + + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 102); } @@ -131,19 +163,31 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { - MAVLINK_ALIGNED_MESSAGE(msg, 24); - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_int32_t(buf, 12, alt); + _mav_put_int16_t(buf, 16, vx); + _mav_put_int16_t(buf, 18, vy); + _mav_put_int16_t(buf, 20, vz); + _mav_put_uint16_t(buf, 22, hdg); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int32_t_by_index(msg, 4, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 8, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 12, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL - put_int16_t_by_index(msg, 16, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 18, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 20, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_uint16_t_by_index(msg, 22, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, buf, 24, 102); +#else + mavlink_global_position_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.hdg = hdg; - mavlink_finalize_message_chan_send(msg, chan, 24, 102); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, (const char *)&packet, 24, 102); +#endif } #endif @@ -158,7 +202,7 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, */ static inline uint32_t mavlink_msg_global_position_int_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -168,7 +212,7 @@ static inline uint32_t mavlink_msg_global_position_int_get_time_boot_ms(const ma */ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -178,7 +222,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lat(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -188,7 +232,7 @@ static inline int32_t mavlink_msg_global_position_int_get_lon(const mavlink_mess */ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 12); + return _MAV_RETURN_int32_t(msg, 12); } /** @@ -198,7 +242,7 @@ static inline int32_t mavlink_msg_global_position_int_get_alt(const mavlink_mess */ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -208,7 +252,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vx(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 18); + return _MAV_RETURN_int16_t(msg, 18); } /** @@ -218,7 +262,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vy(const mavlink_messa */ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 20); + return _MAV_RETURN_int16_t(msg, 20); } /** @@ -228,7 +272,7 @@ static inline int16_t mavlink_msg_global_position_int_get_vz(const mavlink_messa */ static inline uint16_t mavlink_msg_global_position_int_get_hdg(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 22); + return _MAV_RETURN_uint16_t(msg, 22); } /** @@ -249,6 +293,6 @@ static inline void mavlink_msg_global_position_int_decode(const mavlink_message_ global_position_int->vz = mavlink_msg_global_position_int_get_vz(msg); global_position_int->hdg = mavlink_msg_global_position_int_get_hdg(msg); #else - memcpy(global_position_int, MAVLINK_PAYLOAD(msg), 24); + memcpy(global_position_int, _MAV_PAYLOAD(msg), 24); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h index 3f90f5491..8870366b9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_setpoint_int.h @@ -41,13 +41,25 @@ typedef struct __mavlink_global_position_setpoint_int_t static inline uint16_t mavlink_msg_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) { - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_global_position_setpoint_int_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.yaw = yaw; - put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 - put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 - put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) - put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; return mavlink_finalize_message(msg, system_id, component_id, 14, 142); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_ mavlink_message_t* msg, int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) { - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_global_position_setpoint_int_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.yaw = yaw; - put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 - put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 - put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) - put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 142); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_encode(uint8_t s static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) { - MAVLINK_ALIGNED_MESSAGE(msg, 14); - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; - - put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 - put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 - put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) - put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, buf, 14, 142); +#else + mavlink_global_position_setpoint_int_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.yaw = yaw; - mavlink_finalize_message_chan_send(msg, chan, 14, 142); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, 14, 142); +#endif } #endif @@ -126,7 +158,7 @@ static inline void mavlink_msg_global_position_setpoint_int_send(mavlink_channel */ static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -136,7 +168,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_latitude(cons */ static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -146,7 +178,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_longitude(con */ static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -156,7 +188,7 @@ static inline int32_t mavlink_msg_global_position_setpoint_int_get_altitude(cons */ static inline int16_t mavlink_msg_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_global_position_setpoint_int_decode(const mavlink global_position_setpoint_int->altitude = mavlink_msg_global_position_setpoint_int_get_altitude(msg); global_position_setpoint_int->yaw = mavlink_msg_global_position_setpoint_int_get_yaw(msg); #else - memcpy(global_position_setpoint_int, MAVLINK_PAYLOAD(msg), 14); + memcpy(global_position_setpoint_int, _MAV_PAYLOAD(msg), 14); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_global_origin.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_global_origin.h index 24f4782b8..5d74b8d60 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_global_origin.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_global_origin.h @@ -38,12 +38,23 @@ typedef struct __mavlink_gps_global_origin_t static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude) { - msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; - put_int32_t_by_index(msg, 0, latitude); // Latitude (WGS84), expressed as * 1E7 - put_int32_t_by_index(msg, 4, longitude); // Longitude (WGS84), expressed as * 1E7 - put_int32_t_by_index(msg, 8, altitude); // Altitude(WGS84), expressed as * 1000 + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; return mavlink_finalize_message(msg, system_id, component_id, 12, 39); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id mavlink_message_t* msg, int32_t latitude,int32_t longitude,int32_t altitude) { - msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); - put_int32_t_by_index(msg, 0, latitude); // Latitude (WGS84), expressed as * 1E7 - put_int32_t_by_index(msg, 4, longitude); // Longitude (WGS84), expressed as * 1E7 - put_int32_t_by_index(msg, 8, altitude); // Altitude(WGS84), expressed as * 1000 + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 39); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { - MAVLINK_ALIGNED_MESSAGE(msg, 12); - msg->msgid = MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); - put_int32_t_by_index(msg, 0, latitude); // Latitude (WGS84), expressed as * 1E7 - put_int32_t_by_index(msg, 4, longitude); // Longitude (WGS84), expressed as * 1E7 - put_int32_t_by_index(msg, 8, altitude); // Altitude(WGS84), expressed as * 1000 + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, 12, 39); +#else + mavlink_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; - mavlink_finalize_message_chan_send(msg, chan, 12, 39); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, 12, 39); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in */ static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -128,7 +157,7 @@ static inline int32_t mavlink_msg_gps_global_origin_get_latitude(const mavlink_m */ static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -138,7 +167,7 @@ static inline int32_t mavlink_msg_gps_global_origin_get_longitude(const mavlink_ */ static inline int32_t mavlink_msg_gps_global_origin_get_altitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t* gps_global_origin->longitude = mavlink_msg_gps_global_origin_get_longitude(msg); gps_global_origin->altitude = mavlink_msg_gps_global_origin_get_altitude(msg); #else - memcpy(gps_global_origin, MAVLINK_PAYLOAD(msg), 12); + memcpy(gps_global_origin, _MAV_PAYLOAD(msg), 12); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h new file mode 100644 index 000000000..ad65ba9d9 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -0,0 +1,342 @@ +// MESSAGE GPS_RAW PACKING + +#define MAVLINK_MSG_ID_GPS_RAW 32 + +typedef struct __mavlink_gps_raw_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float lat; ///< Latitude in degrees + float lon; ///< Longitude in degrees + float alt; ///< Altitude in meters + float eph; ///< GPS HDOP + float epv; ///< GPS VDOP + float v; ///< GPS ground speed + float hdg; ///< Compass heading in degrees, 0..360 degrees + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible +} mavlink_gps_raw_t; + +#define MAVLINK_MSG_ID_GPS_RAW_LEN 38 +#define MAVLINK_MSG_ID_32_LEN 38 + + + +#define MAVLINK_MESSAGE_INFO_GPS_RAW { \ + "GPS_RAW", \ + 10, \ + { { "usec", MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_t, usec) }, \ + { "lat", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gps_raw_t, lat) }, \ + { "lon", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gps_raw_t, lon) }, \ + { "alt", MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gps_raw_t, alt) }, \ + { "eph", MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gps_raw_t, eph) }, \ + { "epv", MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gps_raw_t, epv) }, \ + { "v", MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gps_raw_t, v) }, \ + { "hdg", MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gps_raw_t, hdg) }, \ + { "fix_type", MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_gps_raw_t, fix_type) }, \ + { "satellites_visible", MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_gps_raw_t, satellites_visible) }, \ + } \ +} + + +/** + * @brief Pack a gps_raw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed + * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[38]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, eph); + _mav_put_float(buf, 24, epv); + _mav_put_float(buf, 28, v); + _mav_put_float(buf, 32, hdg); + _mav_put_uint8_t(buf, 36, fix_type); + _mav_put_uint8_t(buf, 37, satellites_visible); + + memcpy(_MAV_PAYLOAD(msg), buf, 38); +#else + mavlink_gps_raw_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD(msg), &packet, 38); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; + return mavlink_finalize_message(msg, system_id, component_id, 38, 198); +} + +/** + * @brief Pack a gps_raw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed + * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t usec,uint8_t fix_type,float lat,float lon,float alt,float eph,float epv,float v,float hdg,uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[38]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, eph); + _mav_put_float(buf, 24, epv); + _mav_put_float(buf, 28, v); + _mav_put_float(buf, 32, hdg); + _mav_put_uint8_t(buf, 36, fix_type); + _mav_put_uint8_t(buf, 37, satellites_visible); + + memcpy(_MAV_PAYLOAD(msg), buf, 38); +#else + mavlink_gps_raw_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD(msg), &packet, 38); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 38, 198); +} + +/** + * @brief Encode a gps_raw struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_raw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_t* gps_raw) +{ + return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg, gps_raw->satellites_visible); +} + +/** + * @brief Send a gps_raw message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + * @param lat Latitude in degrees + * @param lon Longitude in degrees + * @param alt Altitude in meters + * @param eph GPS HDOP + * @param epv GPS VDOP + * @param v GPS ground speed + * @param hdg Compass heading in degrees, 0..360 degrees + * @param satellites_visible Number of satellites visible + */ +#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, uint8_t satellites_visible) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[38]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, lat); + _mav_put_float(buf, 12, lon); + _mav_put_float(buf, 16, alt); + _mav_put_float(buf, 20, eph); + _mav_put_float(buf, 24, epv); + _mav_put_float(buf, 28, v); + _mav_put_float(buf, 32, hdg); + _mav_put_uint8_t(buf, 36, fix_type); + _mav_put_uint8_t(buf, 37, satellites_visible); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, buf, 38, 198); +#else + mavlink_gps_raw_t packet; + packet.usec = usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.v = v; + packet.hdg = hdg; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW, (const char *)&packet, 38, 198); +#endif +} + +#endif + +// MESSAGE GPS_RAW UNPACKING + + +/** + * @brief Get field usec from gps_raw message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_gps_raw_get_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field fix_type from gps_raw message + * + * @return 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + */ +static inline uint8_t mavlink_msg_gps_raw_get_fix_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field lat from gps_raw message + * + * @return Latitude in degrees + */ +static inline float mavlink_msg_gps_raw_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field lon from gps_raw message + * + * @return Longitude in degrees + */ +static inline float mavlink_msg_gps_raw_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field alt from gps_raw message + * + * @return Altitude in meters + */ +static inline float mavlink_msg_gps_raw_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field eph from gps_raw message + * + * @return GPS HDOP + */ +static inline float mavlink_msg_gps_raw_get_eph(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field epv from gps_raw message + * + * @return GPS VDOP + */ +static inline float mavlink_msg_gps_raw_get_epv(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field v from gps_raw message + * + * @return GPS ground speed + */ +static inline float mavlink_msg_gps_raw_get_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field hdg from gps_raw message + * + * @return Compass heading in degrees, 0..360 degrees + */ +static inline float mavlink_msg_gps_raw_get_hdg(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field satellites_visible from gps_raw message + * + * @return Number of satellites visible + */ +static inline uint8_t mavlink_msg_gps_raw_get_satellites_visible(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + +/** + * @brief Decode a gps_raw message into a struct + * + * @param msg The message to decode + * @param gps_raw C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_raw_decode(const mavlink_message_t* msg, mavlink_gps_raw_t* gps_raw) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_raw->usec = mavlink_msg_gps_raw_get_usec(msg); + gps_raw->lat = mavlink_msg_gps_raw_get_lat(msg); + gps_raw->lon = mavlink_msg_gps_raw_get_lon(msg); + gps_raw->alt = mavlink_msg_gps_raw_get_alt(msg); + gps_raw->eph = mavlink_msg_gps_raw_get_eph(msg); + gps_raw->epv = mavlink_msg_gps_raw_get_epv(msg); + gps_raw->v = mavlink_msg_gps_raw_get_v(msg); + gps_raw->hdg = mavlink_msg_gps_raw_get_hdg(msg); + gps_raw->fix_type = mavlink_msg_gps_raw_get_fix_type(msg); + gps_raw->satellites_visible = mavlink_msg_gps_raw_get_satellites_visible(msg); +#else + memcpy(gps_raw, _MAV_PAYLOAD(msg), 38); +#endif +} 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 eb1c1a065..e6c20309c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -59,19 +59,37 @@ 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 time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) { - msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int32_t_by_index(msg, 8, lat); // Latitude in 1E7 degrees - put_int32_t_by_index(msg, 12, lon); // Longitude in 1E7 degrees - put_int32_t_by_index(msg, 16, alt); // Altitude in 1E3 meters (millimeters) above MSL - put_uint16_t_by_index(msg, 20, eph); // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 22, epv); // GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 24, vel); // GPS ground speed (m/s * 100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 26, cog); // Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - put_uint8_t_by_index(msg, 28, fix_type); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - put_uint8_t_by_index(msg, 29, satellites_visible); // Number of satellites visible. If unknown, set to 255 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + + memcpy(_MAV_PAYLOAD(msg), buf, 30); +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; return mavlink_finalize_message(msg, system_id, component_id, 30, 24); } @@ -97,19 +115,37 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible) { - msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int32_t_by_index(msg, 8, lat); // Latitude in 1E7 degrees - put_int32_t_by_index(msg, 12, lon); // Longitude in 1E7 degrees - put_int32_t_by_index(msg, 16, alt); // Altitude in 1E3 meters (millimeters) above MSL - put_uint16_t_by_index(msg, 20, eph); // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 22, epv); // GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 24, vel); // GPS ground speed (m/s * 100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 26, cog); // Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - put_uint8_t_by_index(msg, 28, fix_type); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - put_uint8_t_by_index(msg, 29, satellites_visible); // Number of satellites visible. If unknown, set to 255 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + + memcpy(_MAV_PAYLOAD(msg), buf, 30); +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 24); } @@ -145,21 +181,35 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) { - MAVLINK_ALIGNED_MESSAGE(msg, 30); - msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int32_t_by_index(msg, 8, lat); // Latitude in 1E7 degrees - put_int32_t_by_index(msg, 12, lon); // Longitude in 1E7 degrees - put_int32_t_by_index(msg, 16, alt); // Altitude in 1E3 meters (millimeters) above MSL - put_uint16_t_by_index(msg, 20, eph); // GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 22, epv); // GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 24, vel); // GPS ground speed (m/s * 100). If unknown, set to: 65535 - put_uint16_t_by_index(msg, 26, cog); // Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - put_uint8_t_by_index(msg, 28, fix_type); // 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - put_uint8_t_by_index(msg, 29, satellites_visible); // Number of satellites visible. If unknown, set to 255 - - mavlink_finalize_message_chan_send(msg, chan, 30, 24); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_uint16_t(buf, 20, eph); + _mav_put_uint16_t(buf, 22, epv); + _mav_put_uint16_t(buf, 24, vel); + _mav_put_uint16_t(buf, 26, cog); + _mav_put_uint8_t(buf, 28, fix_type); + _mav_put_uint8_t(buf, 29, satellites_visible); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, 30, 24); +#else + mavlink_gps_raw_int_t packet; + packet.time_usec = time_usec; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.eph = eph; + packet.epv = epv; + packet.vel = vel; + packet.cog = cog; + packet.fix_type = fix_type; + packet.satellites_visible = satellites_visible; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, 30, 24); +#endif } #endif @@ -174,7 +224,7 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -184,7 +234,7 @@ static inline uint64_t mavlink_msg_gps_raw_int_get_time_usec(const mavlink_messa */ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 28); + return _MAV_RETURN_uint8_t(msg, 28); } /** @@ -194,7 +244,7 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_fix_type(const mavlink_message */ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -204,7 +254,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lat(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 12); + return _MAV_RETURN_int32_t(msg, 12); } /** @@ -214,7 +264,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_lon(const mavlink_message_t* m */ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 16); + return _MAV_RETURN_int32_t(msg, 16); } /** @@ -224,7 +274,7 @@ static inline int32_t mavlink_msg_gps_raw_int_get_alt(const mavlink_message_t* m */ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 20); + return _MAV_RETURN_uint16_t(msg, 20); } /** @@ -234,7 +284,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_eph(const mavlink_message_t* */ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 22); + return _MAV_RETURN_uint16_t(msg, 22); } /** @@ -244,7 +294,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_epv(const mavlink_message_t* */ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 24); } /** @@ -254,7 +304,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_vel(const mavlink_message_t* */ static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 26); + return _MAV_RETURN_uint16_t(msg, 26); } /** @@ -264,7 +314,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_get_cog(const mavlink_message_t* */ static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 29); + return _MAV_RETURN_uint8_t(msg, 29); } /** @@ -287,6 +337,6 @@ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg); #else - memcpy(gps_raw_int, MAVLINK_PAYLOAD(msg), 30); + memcpy(gps_raw_int, _MAV_PAYLOAD(msg), 30); #endif } 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 new file mode 100644 index 000000000..02fa1ca27 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h @@ -0,0 +1,232 @@ +// MESSAGE GPS_SET_GLOBAL_ORIGIN PACKING + +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 + +typedef struct __mavlink_gps_set_global_origin_t +{ + int32_t latitude; ///< global position * 1E7 + int32_t longitude; ///< global position * 1E7 + int32_t altitude; ///< global position * 1000 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID +} mavlink_gps_set_global_origin_t; + +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 +#define MAVLINK_MSG_ID_48_LEN 14 + + + +#define MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN { \ + "GPS_SET_GLOBAL_ORIGIN", \ + 5, \ + { { "latitude", MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_set_global_origin_t, latitude) }, \ + { "longitude", MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_set_global_origin_t, longitude) }, \ + { "altitude", MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_set_global_origin_t, altitude) }, \ + { "target_system", MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gps_set_global_origin_t, target_system) }, \ + { "target_component", MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_gps_set_global_origin_t, target_component) }, \ + } \ +} + + +/** + * @brief Pack a gps_set_global_origin message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_gps_set_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + return mavlink_finalize_message(msg, system_id, component_id, 14, 170); +} + +/** + * @brief Pack a gps_set_global_origin message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,int32_t latitude,int32_t longitude,int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_gps_set_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 170); +} + +/** + * @brief Encode a gps_set_global_origin struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gps_set_global_origin C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_set_global_origin_t* gps_set_global_origin) +{ + return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->latitude, gps_set_global_origin->longitude, gps_set_global_origin->altitude); +} + +/** + * @brief Send a gps_set_global_origin message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param latitude global position * 1E7 + * @param longitude global position * 1E7 + * @param altitude global position * 1000 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, buf, 14, 170); +#else + mavlink_gps_set_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, (const char *)&packet, 14, 170); +#endif +} + +#endif + +// MESSAGE GPS_SET_GLOBAL_ORIGIN UNPACKING + + +/** + * @brief Get field target_system from gps_set_global_origin message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Get field target_component from gps_set_global_origin message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 13); +} + +/** + * @brief Get field latitude from gps_set_global_origin message + * + * @return global position * 1E7 + */ +static inline int32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field longitude from gps_set_global_origin message + * + * @return global position * 1E7 + */ +static inline int32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field altitude from gps_set_global_origin message + * + * @return global position * 1000 + */ +static inline int32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Decode a gps_set_global_origin message into a struct + * + * @param msg The message to decode + * @param gps_set_global_origin C-struct to decode the message contents into + */ +static inline void mavlink_msg_gps_set_global_origin_decode(const mavlink_message_t* msg, mavlink_gps_set_global_origin_t* gps_set_global_origin) +{ +#if MAVLINK_NEED_BYTE_SWAP + gps_set_global_origin->latitude = mavlink_msg_gps_set_global_origin_get_latitude(msg); + gps_set_global_origin->longitude = mavlink_msg_gps_set_global_origin_get_longitude(msg); + gps_set_global_origin->altitude = mavlink_msg_gps_set_global_origin_get_altitude(msg); + gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg); + gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg); +#else + memcpy(gps_set_global_origin, _MAV_PAYLOAD(msg), 14); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index 4b280f6f8..b687beb67 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -51,15 +51,27 @@ typedef struct __mavlink_gps_status_t 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) { - msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - - put_uint8_t_by_index(msg, 0, satellites_visible); // Number of satellites visible - put_uint8_t_array_by_index(msg, 1, satellite_prn, 20); // Global satellite ID - put_uint8_t_array_by_index(msg, 21, satellite_used, 20); // 0: Satellite not used, 1: used for localization - put_uint8_t_array_by_index(msg, 41, satellite_elevation, 20); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - put_uint8_t_array_by_index(msg, 61, satellite_azimuth, 20); // Direction of satellite, 0: 0 deg, 255: 360 deg. - put_uint8_t_array_by_index(msg, 81, satellite_snr, 20); // Signal to noise ratio of satellite +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[101]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD(msg), buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); + memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); + memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD(msg), &packet, 101); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; return mavlink_finalize_message(msg, system_id, component_id, 101, 23); } @@ -81,15 +93,27 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 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) { - msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - - put_uint8_t_by_index(msg, 0, satellites_visible); // Number of satellites visible - put_uint8_t_array_by_index(msg, 1, satellite_prn, 20); // Global satellite ID - put_uint8_t_array_by_index(msg, 21, satellite_used, 20); // 0: Satellite not used, 1: used for localization - put_uint8_t_array_by_index(msg, 41, satellite_elevation, 20); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - put_uint8_t_array_by_index(msg, 61, satellite_azimuth, 20); // Direction of satellite, 0: 0 deg, 255: 360 deg. - put_uint8_t_array_by_index(msg, 81, satellite_snr, 20); // Signal to noise ratio of satellite +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[101]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); + memcpy(_MAV_PAYLOAD(msg), buf, 101); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); + memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); + memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD(msg), &packet, 101); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 101, 23); } @@ -121,17 +145,25 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t 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_ALIGNED_MESSAGE(msg, 101); - msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - - put_uint8_t_by_index(msg, 0, satellites_visible); // Number of satellites visible - put_uint8_t_array_by_index(msg, 1, satellite_prn, 20); // Global satellite ID - put_uint8_t_array_by_index(msg, 21, satellite_used, 20); // 0: Satellite not used, 1: used for localization - put_uint8_t_array_by_index(msg, 41, satellite_elevation, 20); // Elevation (0: right on top of receiver, 90: on the horizon) of satellite - put_uint8_t_array_by_index(msg, 61, satellite_azimuth, 20); // Direction of satellite, 0: 0 deg, 255: 360 deg. - put_uint8_t_array_by_index(msg, 81, satellite_snr, 20); // Signal to noise ratio of satellite - - mavlink_finalize_message_chan_send(msg, chan, 101, 23); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[101]; + _mav_put_uint8_t(buf, 0, satellites_visible); + _mav_put_uint8_t_array(buf, 1, satellite_prn, 20); + _mav_put_uint8_t_array(buf, 21, satellite_used, 20); + _mav_put_uint8_t_array(buf, 41, satellite_elevation, 20); + _mav_put_uint8_t_array(buf, 61, satellite_azimuth, 20); + _mav_put_uint8_t_array(buf, 81, satellite_snr, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, buf, 101, 23); +#else + mavlink_gps_status_t packet; + packet.satellites_visible = satellites_visible; + memcpy(packet.satellite_prn, satellite_prn, sizeof(uint8_t)*20); + memcpy(packet.satellite_used, satellite_used, sizeof(uint8_t)*20); + memcpy(packet.satellite_elevation, satellite_elevation, sizeof(uint8_t)*20); + memcpy(packet.satellite_azimuth, satellite_azimuth, sizeof(uint8_t)*20); + memcpy(packet.satellite_snr, satellite_snr, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_STATUS, (const char *)&packet, 101, 23); +#endif } #endif @@ -146,7 +178,7 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -156,7 +188,7 @@ static inline uint8_t mavlink_msg_gps_status_get_satellites_visible(const mavlin */ static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_message_t* msg, uint8_t *satellite_prn) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_prn, 20, 1); + return _MAV_RETURN_uint8_t_array(msg, satellite_prn, 20, 1); } /** @@ -166,7 +198,7 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_prn(const mavlink_me */ static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_message_t* msg, uint8_t *satellite_used) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_used, 20, 21); + return _MAV_RETURN_uint8_t_array(msg, satellite_used, 20, 21); } /** @@ -176,7 +208,7 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_used(const mavlink_m */ static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavlink_message_t* msg, uint8_t *satellite_elevation) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_elevation, 20, 41); + return _MAV_RETURN_uint8_t_array(msg, satellite_elevation, 20, 41); } /** @@ -186,7 +218,7 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_elevation(const mavl */ static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlink_message_t* msg, uint8_t *satellite_azimuth) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_azimuth, 20, 61); + return _MAV_RETURN_uint8_t_array(msg, satellite_azimuth, 20, 61); } /** @@ -196,7 +228,7 @@ static inline uint16_t mavlink_msg_gps_status_get_satellite_azimuth(const mavlin */ static inline uint16_t mavlink_msg_gps_status_get_satellite_snr(const mavlink_message_t* msg, uint8_t *satellite_snr) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, satellite_snr, 20, 81); + return _MAV_RETURN_uint8_t_array(msg, satellite_snr, 20, 81); } /** @@ -215,6 +247,6 @@ static inline void mavlink_msg_gps_status_decode(const mavlink_message_t* msg, m mavlink_msg_gps_status_get_satellite_azimuth(msg, gps_status->satellite_azimuth); mavlink_msg_gps_status_get_satellite_snr(msg, gps_status->satellite_snr); #else - memcpy(gps_status, MAVLINK_PAYLOAD(msg), 101); + memcpy(gps_status, _MAV_PAYLOAD(msg), 101); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 44315c62b..c0dfab368 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -46,15 +46,29 @@ 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, uint8_t base_mode, uint16_t custom_mode, uint8_t system_status) { - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - put_uint16_t_by_index(msg, 0, custom_mode); // Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. - put_uint8_t_by_index(msg, 2, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - put_uint8_t_by_index(msg, 3, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM - put_uint8_t_by_index(msg, 4, base_mode); // System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 5, system_status); // System status flag, see MAV_STATUS ENUM - put_uint8_t_by_index(msg, 6, 3); // MAVLink version +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[7]; + _mav_put_uint16_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, autopilot); + _mav_put_uint8_t(buf, 4, base_mode); + _mav_put_uint8_t(buf, 5, system_status); + _mav_put_uint8_t(buf, 6, 3); + + memcpy(_MAV_PAYLOAD(msg), buf, 7); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + + memcpy(_MAV_PAYLOAD(msg), &packet, 7); +#endif + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; return mavlink_finalize_message(msg, system_id, component_id, 7, 88); } @@ -75,15 +89,29 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ mavlink_message_t* msg, uint8_t type,uint8_t autopilot,uint8_t base_mode,uint16_t custom_mode,uint8_t system_status) { - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - put_uint16_t_by_index(msg, 0, custom_mode); // Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. - put_uint8_t_by_index(msg, 2, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - put_uint8_t_by_index(msg, 3, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM - put_uint8_t_by_index(msg, 4, base_mode); // System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 5, system_status); // System status flag, see MAV_STATUS ENUM - put_uint8_t_by_index(msg, 6, 3); // MAVLink version +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[7]; + _mav_put_uint16_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, autopilot); + _mav_put_uint8_t(buf, 4, base_mode); + _mav_put_uint8_t(buf, 5, system_status); + _mav_put_uint8_t(buf, 6, 3); + + memcpy(_MAV_PAYLOAD(msg), buf, 7); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + + memcpy(_MAV_PAYLOAD(msg), &packet, 7); +#endif + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 7, 88); } @@ -114,17 +142,27 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t base_mode, uint16_t custom_mode, uint8_t system_status) { - MAVLINK_ALIGNED_MESSAGE(msg, 7); - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - put_uint16_t_by_index(msg, 0, custom_mode); // Navigation mode bitfield, see MAV_AUTOPILOT_CUSTOM_MODE ENUM for some examples. This field is autopilot-specific. - put_uint8_t_by_index(msg, 2, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - put_uint8_t_by_index(msg, 3, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM - put_uint8_t_by_index(msg, 4, base_mode); // System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 5, system_status); // System status flag, see MAV_STATUS ENUM - put_uint8_t_by_index(msg, 6, 3); // MAVLink version - - mavlink_finalize_message_chan_send(msg, chan, 7, 88); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[7]; + _mav_put_uint16_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, autopilot); + _mav_put_uint8_t(buf, 4, base_mode); + _mav_put_uint8_t(buf, 5, system_status); + _mav_put_uint8_t(buf, 6, 3); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, 7, 88); +#else + mavlink_heartbeat_t packet; + packet.custom_mode = custom_mode; + packet.type = type; + packet.autopilot = autopilot; + packet.base_mode = base_mode; + packet.system_status = system_status; + packet.mavlink_version = 3; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, 7, 88); +#endif } #endif @@ -139,7 +177,7 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -149,7 +187,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -159,7 +197,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -169,7 +207,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_base_mode(const mavlink_message_ */ static inline uint16_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -179,7 +217,7 @@ static inline uint16_t mavlink_msg_heartbeat_get_custom_mode(const mavlink_messa */ static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -189,7 +227,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_system_status(const mavlink_mess */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -208,6 +246,6 @@ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, ma heartbeat->system_status = mavlink_msg_heartbeat_get_system_status(msg); heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); #else - memcpy(heartbeat, MAVLINK_PAYLOAD(msg), 7); + memcpy(heartbeat, _MAV_PAYLOAD(msg), 7); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h index e121317fb..d42172c98 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h @@ -62,20 +62,39 @@ typedef struct __mavlink_hil_controls_t static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) { - msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, roll_ailerons); // Control output -1 .. 1 - put_float_by_index(msg, 12, pitch_elevator); // Control output -1 .. 1 - put_float_by_index(msg, 16, yaw_rudder); // Control output -1 .. 1 - put_float_by_index(msg, 20, throttle); // Throttle 0 .. 1 - put_float_by_index(msg, 24, aux1); // Aux 1, -1 .. 1 - put_float_by_index(msg, 28, aux2); // Aux 2, -1 .. 1 - put_float_by_index(msg, 32, aux3); // Aux 3, -1 .. 1 - put_float_by_index(msg, 36, aux4); // Aux 4, -1 .. 1 - put_uint8_t_by_index(msg, 40, mode); // System mode (MAV_MODE) - put_uint8_t_by_index(msg, 41, nav_mode); // Navigation mode (MAV_NAV_MODE) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; return mavlink_finalize_message(msg, system_id, component_id, 42, 63); } @@ -102,20 +121,39 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint64_t time_usec,float roll_ailerons,float pitch_elevator,float yaw_rudder,float throttle,float aux1,float aux2,float aux3,float aux4,uint8_t mode,uint8_t nav_mode) { - msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, roll_ailerons); // Control output -1 .. 1 - put_float_by_index(msg, 12, pitch_elevator); // Control output -1 .. 1 - put_float_by_index(msg, 16, yaw_rudder); // Control output -1 .. 1 - put_float_by_index(msg, 20, throttle); // Throttle 0 .. 1 - put_float_by_index(msg, 24, aux1); // Aux 1, -1 .. 1 - put_float_by_index(msg, 28, aux2); // Aux 2, -1 .. 1 - put_float_by_index(msg, 32, aux3); // Aux 3, -1 .. 1 - put_float_by_index(msg, 36, aux4); // Aux 4, -1 .. 1 - put_uint8_t_by_index(msg, 40, mode); // System mode (MAV_MODE) - put_uint8_t_by_index(msg, 41, nav_mode); // Navigation mode (MAV_NAV_MODE) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 63); } @@ -152,22 +190,37 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_usec, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) { - MAVLINK_ALIGNED_MESSAGE(msg, 42); - msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, roll_ailerons); // Control output -1 .. 1 - put_float_by_index(msg, 12, pitch_elevator); // Control output -1 .. 1 - put_float_by_index(msg, 16, yaw_rudder); // Control output -1 .. 1 - put_float_by_index(msg, 20, throttle); // Throttle 0 .. 1 - put_float_by_index(msg, 24, aux1); // Aux 1, -1 .. 1 - put_float_by_index(msg, 28, aux2); // Aux 2, -1 .. 1 - put_float_by_index(msg, 32, aux3); // Aux 3, -1 .. 1 - put_float_by_index(msg, 36, aux4); // Aux 4, -1 .. 1 - put_uint8_t_by_index(msg, 40, mode); // System mode (MAV_MODE) - put_uint8_t_by_index(msg, 41, nav_mode); // Navigation mode (MAV_NAV_MODE) - - mavlink_finalize_message_chan_send(msg, chan, 42, 63); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll_ailerons); + _mav_put_float(buf, 12, pitch_elevator); + _mav_put_float(buf, 16, yaw_rudder); + _mav_put_float(buf, 20, throttle); + _mav_put_float(buf, 24, aux1); + _mav_put_float(buf, 28, aux2); + _mav_put_float(buf, 32, aux3); + _mav_put_float(buf, 36, aux4); + _mav_put_uint8_t(buf, 40, mode); + _mav_put_uint8_t(buf, 41, nav_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, buf, 42, 63); +#else + mavlink_hil_controls_t packet; + packet.time_usec = time_usec; + packet.roll_ailerons = roll_ailerons; + packet.pitch_elevator = pitch_elevator; + packet.yaw_rudder = yaw_rudder; + packet.throttle = throttle; + packet.aux1 = aux1; + packet.aux2 = aux2; + packet.aux3 = aux3; + packet.aux4 = aux4; + packet.mode = mode; + packet.nav_mode = nav_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_CONTROLS, (const char *)&packet, 42, 63); +#endif } #endif @@ -182,7 +235,7 @@ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_hil_controls_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -192,7 +245,7 @@ static inline uint64_t mavlink_msg_hil_controls_get_time_usec(const mavlink_mess */ static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -202,7 +255,7 @@ static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_mes */ static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -212,7 +265,7 @@ static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_me */ static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -222,7 +275,7 @@ static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_messag */ static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -232,7 +285,7 @@ static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_ */ static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -242,7 +295,7 @@ static inline float mavlink_msg_hil_controls_get_aux1(const mavlink_message_t* m */ static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -252,7 +305,7 @@ static inline float mavlink_msg_hil_controls_get_aux2(const mavlink_message_t* m */ static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 32); + return _MAV_RETURN_float(msg, 32); } /** @@ -262,7 +315,7 @@ static inline float mavlink_msg_hil_controls_get_aux3(const mavlink_message_t* m */ static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 36); + return _MAV_RETURN_float(msg, 36); } /** @@ -272,7 +325,7 @@ static inline float mavlink_msg_hil_controls_get_aux4(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 40); + return _MAV_RETURN_uint8_t(msg, 40); } /** @@ -282,7 +335,7 @@ static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* */ static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 41); + return _MAV_RETURN_uint8_t(msg, 41); } /** @@ -306,6 +359,6 @@ static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg); hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg); #else - memcpy(hil_controls, MAVLINK_PAYLOAD(msg), 42); + memcpy(hil_controls, _MAV_PAYLOAD(msg), 42); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h index 40378c167..ec936f03a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_rc_inputs_raw.h @@ -71,23 +71,45 @@ typedef struct __mavlink_hil_rc_inputs_raw_t static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) { - msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_uint16_t_by_index(msg, 8, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 10, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 12, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 14, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 16, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 18, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 20, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 22, chan8_raw); // RC channel 8 value, in microseconds - put_uint16_t_by_index(msg, 24, chan9_raw); // RC channel 9 value, in microseconds - put_uint16_t_by_index(msg, 26, chan10_raw); // RC channel 10 value, in microseconds - put_uint16_t_by_index(msg, 28, chan11_raw); // RC channel 11 value, in microseconds - put_uint16_t_by_index(msg, 30, chan12_raw); // RC channel 12 value, in microseconds - put_uint8_t_by_index(msg, 32, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 33); +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 33); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; return mavlink_finalize_message(msg, system_id, component_id, 33, 54); } @@ -117,23 +139,45 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id mavlink_message_t* msg, uint64_t time_usec,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint8_t rssi) { - msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_uint16_t_by_index(msg, 8, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 10, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 12, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 14, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 16, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 18, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 20, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 22, chan8_raw); // RC channel 8 value, in microseconds - put_uint16_t_by_index(msg, 24, chan9_raw); // RC channel 9 value, in microseconds - put_uint16_t_by_index(msg, 26, chan10_raw); // RC channel 10 value, in microseconds - put_uint16_t_by_index(msg, 28, chan11_raw); // RC channel 11 value, in microseconds - put_uint16_t_by_index(msg, 30, chan12_raw); // RC channel 12 value, in microseconds - put_uint8_t_by_index(msg, 32, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 33); +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 33); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 54); } @@ -173,25 +217,43 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_encode(uint8_t system_id, u static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint8_t rssi) { - MAVLINK_ALIGNED_MESSAGE(msg, 33); - msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_uint16_t_by_index(msg, 8, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 10, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 12, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 14, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 16, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 18, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 20, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 22, chan8_raw); // RC channel 8 value, in microseconds - put_uint16_t_by_index(msg, 24, chan9_raw); // RC channel 9 value, in microseconds - put_uint16_t_by_index(msg, 26, chan10_raw); // RC channel 10 value, in microseconds - put_uint16_t_by_index(msg, 28, chan11_raw); // RC channel 11 value, in microseconds - put_uint16_t_by_index(msg, 30, chan12_raw); // RC channel 12 value, in microseconds - put_uint8_t_by_index(msg, 32, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% - - mavlink_finalize_message_chan_send(msg, chan, 33, 54); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, chan1_raw); + _mav_put_uint16_t(buf, 10, chan2_raw); + _mav_put_uint16_t(buf, 12, chan3_raw); + _mav_put_uint16_t(buf, 14, chan4_raw); + _mav_put_uint16_t(buf, 16, chan5_raw); + _mav_put_uint16_t(buf, 18, chan6_raw); + _mav_put_uint16_t(buf, 20, chan7_raw); + _mav_put_uint16_t(buf, 22, chan8_raw); + _mav_put_uint16_t(buf, 24, chan9_raw); + _mav_put_uint16_t(buf, 26, chan10_raw); + _mav_put_uint16_t(buf, 28, chan11_raw); + _mav_put_uint16_t(buf, 30, chan12_raw); + _mav_put_uint8_t(buf, 32, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, buf, 33, 54); +#else + mavlink_hil_rc_inputs_raw_t packet; + packet.time_usec = time_usec; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW, (const char *)&packet, 33, 54); +#endif } #endif @@ -206,7 +268,7 @@ static inline void mavlink_msg_hil_rc_inputs_raw_send(mavlink_channel_t chan, ui */ static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -216,7 +278,7 @@ static inline uint64_t mavlink_msg_hil_rc_inputs_raw_get_time_usec(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -226,7 +288,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan1_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -236,7 +298,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan2_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -246,7 +308,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan3_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -256,7 +318,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan4_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -266,7 +328,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan5_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** @@ -276,7 +338,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan6_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 20); + return _MAV_RETURN_uint16_t(msg, 20); } /** @@ -286,7 +348,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan7_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 22); + return _MAV_RETURN_uint16_t(msg, 22); } /** @@ -296,7 +358,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan8_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 24); } /** @@ -306,7 +368,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan9_raw(const mavlink */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 26); + return _MAV_RETURN_uint16_t(msg, 26); } /** @@ -316,7 +378,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan10_raw(const mavlin */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 28); + return _MAV_RETURN_uint16_t(msg, 28); } /** @@ -326,7 +388,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan11_raw(const mavlin */ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 30); + return _MAV_RETURN_uint16_t(msg, 30); } /** @@ -336,7 +398,7 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(const mavlin */ static inline uint8_t mavlink_msg_hil_rc_inputs_raw_get_rssi(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 32); + return _MAV_RETURN_uint8_t(msg, 32); } /** @@ -363,6 +425,6 @@ static inline void mavlink_msg_hil_rc_inputs_raw_decode(const mavlink_message_t* hil_rc_inputs_raw->chan12_raw = mavlink_msg_hil_rc_inputs_raw_get_chan12_raw(msg); hil_rc_inputs_raw->rssi = mavlink_msg_hil_rc_inputs_raw_get_rssi(msg); #else - memcpy(hil_rc_inputs_raw, MAVLINK_PAYLOAD(msg), 33); + memcpy(hil_rc_inputs_raw, _MAV_PAYLOAD(msg), 33); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h index 65ce7af07..45fc98912 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h @@ -77,25 +77,49 @@ typedef struct __mavlink_hil_state_t static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) { - msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, roll); // Roll angle (rad) - put_float_by_index(msg, 12, pitch); // Pitch angle (rad) - put_float_by_index(msg, 16, yaw); // Yaw angle (rad) - put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) - put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) - put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) - put_int32_t_by_index(msg, 32, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 36, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 40, alt); // Altitude in meters, expressed as * 1000 (millimeters) - put_int16_t_by_index(msg, 44, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 46, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 48, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 50, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 52, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 54, zacc); // Z acceleration (mg) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[56]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD(msg), buf, 56); +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD(msg), &packet, 56); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; return mavlink_finalize_message(msg, system_id, component_id, 56, 183); } @@ -127,25 +151,49 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_ mavlink_message_t* msg, uint64_t time_usec,float roll,float pitch,float yaw,float rollspeed,float pitchspeed,float yawspeed,int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,int16_t xacc,int16_t yacc,int16_t zacc) { - msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, roll); // Roll angle (rad) - put_float_by_index(msg, 12, pitch); // Pitch angle (rad) - put_float_by_index(msg, 16, yaw); // Yaw angle (rad) - put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) - put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) - put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) - put_int32_t_by_index(msg, 32, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 36, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 40, alt); // Altitude in meters, expressed as * 1000 (millimeters) - put_int16_t_by_index(msg, 44, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 46, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 48, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 50, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 52, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 54, zacc); // Z acceleration (mg) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[56]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + memcpy(_MAV_PAYLOAD(msg), buf, 56); +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + memcpy(_MAV_PAYLOAD(msg), &packet, 56); +#endif + msg->msgid = MAVLINK_MSG_ID_HIL_STATE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 56, 183); } @@ -187,27 +235,47 @@ static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t c static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) { - MAVLINK_ALIGNED_MESSAGE(msg, 56); - msg->msgid = MAVLINK_MSG_ID_HIL_STATE; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, roll); // Roll angle (rad) - put_float_by_index(msg, 12, pitch); // Pitch angle (rad) - put_float_by_index(msg, 16, yaw); // Yaw angle (rad) - put_float_by_index(msg, 20, rollspeed); // Roll angular speed (rad/s) - put_float_by_index(msg, 24, pitchspeed); // Pitch angular speed (rad/s) - put_float_by_index(msg, 28, yawspeed); // Yaw angular speed (rad/s) - put_int32_t_by_index(msg, 32, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 36, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 40, alt); // Altitude in meters, expressed as * 1000 (millimeters) - put_int16_t_by_index(msg, 44, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 46, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 48, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 50, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 52, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 54, zacc); // Z acceleration (mg) - - mavlink_finalize_message_chan_send(msg, chan, 56, 183); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[56]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, roll); + _mav_put_float(buf, 12, pitch); + _mav_put_float(buf, 16, yaw); + _mav_put_float(buf, 20, rollspeed); + _mav_put_float(buf, 24, pitchspeed); + _mav_put_float(buf, 28, yawspeed); + _mav_put_int32_t(buf, 32, lat); + _mav_put_int32_t(buf, 36, lon); + _mav_put_int32_t(buf, 40, alt); + _mav_put_int16_t(buf, 44, vx); + _mav_put_int16_t(buf, 46, vy); + _mav_put_int16_t(buf, 48, vz); + _mav_put_int16_t(buf, 50, xacc); + _mav_put_int16_t(buf, 52, yacc); + _mav_put_int16_t(buf, 54, zacc); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, buf, 56, 183); +#else + mavlink_hil_state_t packet; + packet.time_usec = time_usec; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_STATE, (const char *)&packet, 56, 183); +#endif } #endif @@ -222,7 +290,7 @@ static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t t */ static inline uint64_t mavlink_msg_hil_state_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -232,7 +300,7 @@ static inline uint64_t mavlink_msg_hil_state_get_time_usec(const mavlink_message */ static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -242,7 +310,7 @@ static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg) */ static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -252,7 +320,7 @@ static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg */ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -262,7 +330,7 @@ static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg) */ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -272,7 +340,7 @@ static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* */ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -282,7 +350,7 @@ static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t */ static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -292,7 +360,7 @@ static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* */ static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 32); + return _MAV_RETURN_int32_t(msg, 32); } /** @@ -302,7 +370,7 @@ static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg */ static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 36); + return _MAV_RETURN_int32_t(msg, 36); } /** @@ -312,7 +380,7 @@ static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg */ static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 40); + return _MAV_RETURN_int32_t(msg, 40); } /** @@ -322,7 +390,7 @@ static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 44); + return _MAV_RETURN_int16_t(msg, 44); } /** @@ -332,7 +400,7 @@ static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 46); + return _MAV_RETURN_int16_t(msg, 46); } /** @@ -342,7 +410,7 @@ static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 48); + return _MAV_RETURN_int16_t(msg, 48); } /** @@ -352,7 +420,7 @@ static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 50); + return _MAV_RETURN_int16_t(msg, 50); } /** @@ -362,7 +430,7 @@ static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* ms */ static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 52); + return _MAV_RETURN_int16_t(msg, 52); } /** @@ -372,7 +440,7 @@ static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* ms */ static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 54); + return _MAV_RETURN_int16_t(msg, 54); } /** @@ -401,6 +469,6 @@ static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, ma hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg); hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg); #else - memcpy(hil_state, MAVLINK_PAYLOAD(msg), 56); + memcpy(hil_state, _MAV_PAYLOAD(msg), 56); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index ce36bef81..dfdf965dc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -50,16 +50,31 @@ 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, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) { - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); + + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_local_position_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, x); // X Position - put_float_by_index(msg, 8, y); // Y Position - put_float_by_index(msg, 12, z); // Z Position - put_float_by_index(msg, 16, vx); // X Speed - put_float_by_index(msg, 20, vy); // Y Speed - put_float_by_index(msg, 24, vz); // Z Speed + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; return mavlink_finalize_message(msg, system_id, component_id, 28, 231); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, u mavlink_message_t* msg, uint32_t time_boot_ms,float x,float y,float z,float vx,float vy,float vz) { - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, x); // X Position - put_float_by_index(msg, 8, y); // Y Position - put_float_by_index(msg, 12, z); // Z Position - put_float_by_index(msg, 16, vx); // X Speed - put_float_by_index(msg, 20, vy); // Y Speed - put_float_by_index(msg, 24, vz); // Z Speed + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_local_position_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 231); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint32_t time_boot_ms, float x, float y, float z, float vx, float vy, float vz) { - MAVLINK_ALIGNED_MESSAGE(msg, 28); - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, x); + _mav_put_float(buf, 8, y); + _mav_put_float(buf, 12, z); + _mav_put_float(buf, 16, vx); + _mav_put_float(buf, 20, vy); + _mav_put_float(buf, 24, vz); - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, x); // X Position - put_float_by_index(msg, 8, y); // Y Position - put_float_by_index(msg, 12, z); // Z Position - put_float_by_index(msg, 16, vx); // X Speed - put_float_by_index(msg, 20, vy); // Y Speed - put_float_by_index(msg, 24, vz); // Z Speed + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION, buf, 28, 231); +#else + mavlink_local_position_t packet; + packet.time_boot_ms = time_boot_ms; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; - mavlink_finalize_message_chan_send(msg, chan, 28, 231); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION, (const char *)&packet, 28, 231); +#endif } #endif @@ -150,7 +191,7 @@ static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint3 */ static inline uint32_t mavlink_msg_local_position_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -160,7 +201,7 @@ static inline uint32_t mavlink_msg_local_position_get_time_boot_ms(const mavlink */ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -170,7 +211,7 @@ static inline float mavlink_msg_local_position_get_x(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -180,7 +221,7 @@ static inline float mavlink_msg_local_position_get_y(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -190,7 +231,7 @@ static inline float mavlink_msg_local_position_get_z(const mavlink_message_t* ms */ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -200,7 +241,7 @@ static inline float mavlink_msg_local_position_get_vx(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -210,7 +251,7 @@ static inline float mavlink_msg_local_position_get_vy(const mavlink_message_t* m */ static inline float mavlink_msg_local_position_get_vz(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_local_position_decode(const mavlink_message_t* ms local_position->vy = mavlink_msg_local_position_get_vy(msg); local_position->vz = mavlink_msg_local_position_get_vz(msg); #else - memcpy(local_position, MAVLINK_PAYLOAD(msg), 28); + memcpy(local_position, _MAV_PAYLOAD(msg), 28); #endif } 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 735b1420c..d48748808 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -41,13 +41,25 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // Desired yaw angle + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; return mavlink_finalize_message(msg, system_id, component_id, 16, 50); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys mavlink_message_t* msg, float x,float y,float z,float yaw) { - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // Desired yaw angle + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 50); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { - MAVLINK_ALIGNED_MESSAGE(msg, 16); - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // Desired yaw angle +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, buf, 16, 50); +#else + mavlink_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; - mavlink_finalize_message_chan_send(msg, chan, 16, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, (const char *)&packet, 16, 50); +#endif } #endif @@ -126,7 +158,7 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch */ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -136,7 +168,7 @@ static inline float mavlink_msg_local_position_setpoint_get_x(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -146,7 +178,7 @@ static inline float mavlink_msg_local_position_setpoint_get_y(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -156,7 +188,7 @@ static inline float mavlink_msg_local_position_setpoint_get_z(const mavlink_mess */ static inline float mavlink_msg_local_position_setpoint_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_local_position_setpoint_decode(const mavlink_mess local_position_setpoint->z = mavlink_msg_local_position_setpoint_get_z(msg); local_position_setpoint->yaw = mavlink_msg_local_position_setpoint_get_yaw(msg); #else - memcpy(local_position_setpoint, MAVLINK_PAYLOAD(msg), 16); + memcpy(local_position_setpoint, _MAV_PAYLOAD(msg), 16); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index e0e88e05a..4bd80e4b7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -56,18 +56,35 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + memcpy(_MAV_PAYLOAD(msg), buf, 21); +#else + mavlink_manual_control_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target = target; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; - put_float_by_index(msg, 0, roll); // roll - put_float_by_index(msg, 4, pitch); // pitch - put_float_by_index(msg, 8, yaw); // yaw - put_float_by_index(msg, 12, thrust); // thrust - put_uint8_t_by_index(msg, 16, target); // The system to be controlled - put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 - put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 - put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 - put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; return mavlink_finalize_message(msg, system_id, component_id, 21, 52); } @@ -92,18 +109,35 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u 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) { - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + memcpy(_MAV_PAYLOAD(msg), buf, 21); +#else + mavlink_manual_control_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target = target; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; - put_float_by_index(msg, 0, roll); // roll - put_float_by_index(msg, 4, pitch); // pitch - put_float_by_index(msg, 8, yaw); // yaw - put_float_by_index(msg, 12, thrust); // thrust - put_uint8_t_by_index(msg, 16, target); // The system to be controlled - put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 - put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 - put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 - put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 52); } @@ -138,20 +172,33 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { - MAVLINK_ALIGNED_MESSAGE(msg, 21); - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); - put_float_by_index(msg, 0, roll); // roll - put_float_by_index(msg, 4, pitch); // pitch - put_float_by_index(msg, 8, yaw); // yaw - put_float_by_index(msg, 12, thrust); // thrust - put_uint8_t_by_index(msg, 16, target); // The system to be controlled - put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 - put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 - put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 - put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 - - mavlink_finalize_message_chan_send(msg, chan, 21, 52); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, buf, 21, 52); +#else + mavlink_manual_control_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target = target; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MANUAL_CONTROL, (const char *)&packet, 21, 52); +#endif } #endif @@ -166,7 +213,7 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -176,7 +223,7 @@ static inline uint8_t mavlink_msg_manual_control_get_target(const mavlink_messag */ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -186,7 +233,7 @@ static inline float mavlink_msg_manual_control_get_roll(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -196,7 +243,7 @@ static inline float mavlink_msg_manual_control_get_pitch(const mavlink_message_t */ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -206,7 +253,7 @@ static inline float mavlink_msg_manual_control_get_yaw(const mavlink_message_t* */ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -216,7 +263,7 @@ static inline float mavlink_msg_manual_control_get_thrust(const mavlink_message_ */ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -226,7 +273,7 @@ static inline uint8_t mavlink_msg_manual_control_get_roll_manual(const mavlink_m */ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 18); + return _MAV_RETURN_uint8_t(msg, 18); } /** @@ -236,7 +283,7 @@ static inline uint8_t mavlink_msg_manual_control_get_pitch_manual(const mavlink_ */ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 19); + return _MAV_RETURN_uint8_t(msg, 19); } /** @@ -246,7 +293,7 @@ static inline uint8_t mavlink_msg_manual_control_get_yaw_manual(const mavlink_me */ static inline uint8_t mavlink_msg_manual_control_get_thrust_manual(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -268,6 +315,6 @@ static inline void mavlink_msg_manual_control_decode(const mavlink_message_t* ms manual_control->yaw_manual = mavlink_msg_manual_control_get_yaw_manual(msg); manual_control->thrust_manual = mavlink_msg_manual_control_get_thrust_manual(msg); #else - memcpy(manual_control, MAVLINK_PAYLOAD(msg), 21); + memcpy(manual_control, _MAV_PAYLOAD(msg), 21); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h index 233a68faa..1cb29ed01 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -41,13 +41,23 @@ typedef struct __mavlink_memory_vect_t static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) { - msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; - - put_uint16_t_by_index(msg, 0, address); // Starting address of the debug variables - put_uint8_t_by_index(msg, 2, ver); // Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - put_uint8_t_by_index(msg, 3, type); // Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - put_int8_t_array_by_index(msg, 4, value, 32); // Memory contents at specified address +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 36); +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + memcpy(packet.value, value, sizeof(int8_t)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; return mavlink_finalize_message(msg, system_id, component_id, 36, 204); } @@ -67,13 +77,23 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, uint16_t address,uint8_t ver,uint8_t type,const int8_t *value) { - msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; - - put_uint16_t_by_index(msg, 0, address); // Starting address of the debug variables - put_uint8_t_by_index(msg, 2, ver); // Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - put_uint8_t_by_index(msg, 3, type); // Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - put_int8_t_array_by_index(msg, 4, value, 32); // Memory contents at specified address +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 36); +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + memcpy(packet.value, value, sizeof(int8_t)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 204); } @@ -103,15 +123,21 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t address, uint8_t ver, uint8_t type, const int8_t *value) { - MAVLINK_ALIGNED_MESSAGE(msg, 36); - msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; - - put_uint16_t_by_index(msg, 0, address); // Starting address of the debug variables - put_uint8_t_by_index(msg, 2, ver); // Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - put_uint8_t_by_index(msg, 3, type); // Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 - put_int8_t_array_by_index(msg, 4, value, 32); // Memory contents at specified address - - mavlink_finalize_message_chan_send(msg, chan, 36, 204); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_uint16_t(buf, 0, address); + _mav_put_uint8_t(buf, 2, ver); + _mav_put_uint8_t(buf, 3, type); + _mav_put_int8_t_array(buf, 4, value, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, buf, 36, 204); +#else + mavlink_memory_vect_t packet; + packet.address = address; + packet.ver = ver; + packet.type = type; + memcpy(packet.value, value, sizeof(int8_t)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MEMORY_VECT, (const char *)&packet, 36, 204); +#endif } #endif @@ -126,7 +152,7 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t */ static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -136,7 +162,7 @@ static inline uint16_t mavlink_msg_memory_vect_get_address(const mavlink_message */ static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -146,7 +172,7 @@ static inline uint8_t mavlink_msg_memory_vect_get_ver(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -156,7 +182,7 @@ static inline uint8_t mavlink_msg_memory_vect_get_type(const mavlink_message_t* */ static inline uint16_t mavlink_msg_memory_vect_get_value(const mavlink_message_t* msg, int8_t *value) { - return MAVLINK_MSG_RETURN_int8_t_array(msg, value, 32, 4); + return _MAV_RETURN_int8_t_array(msg, value, 32, 4); } /** @@ -173,6 +199,6 @@ static inline void mavlink_msg_memory_vect_decode(const mavlink_message_t* msg, memory_vect->type = mavlink_msg_memory_vect_get_type(msg); mavlink_msg_memory_vect_get_value(msg, memory_vect->value); #else - memcpy(memory_vect, MAVLINK_PAYLOAD(msg), 36); + memcpy(memory_vect, _MAV_PAYLOAD(msg), 36); #endif } 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 38727f423..510c55653 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -38,12 +38,21 @@ 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, uint32_t time_boot_ms, const char *name, float value) { - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, value); // Floating point value - put_char_array_by_index(msg, 8, name, 10); // Name of the debug variable +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; return mavlink_finalize_message(msg, system_id, component_id, 18, 170); } @@ -62,12 +71,21 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id mavlink_message_t* msg, uint32_t time_boot_ms,const char *name,float value) { - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, value); // Floating point value - put_char_array_by_index(msg, 8, name, 10); // Name of the debug variable +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 170); } @@ -96,14 +114,19 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, float value) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_float_by_index(msg, 4, value); // Floating point value - put_char_array_by_index(msg, 8, name, 10); // Name of the debug variable - - mavlink_finalize_message_chan_send(msg, chan, 18, 170); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, buf, 18, 170); +#else + mavlink_named_value_float_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, (const char *)&packet, 18, 170); +#endif } #endif @@ -118,7 +141,7 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, ui */ static inline uint32_t mavlink_msg_named_value_float_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -128,7 +151,7 @@ static inline uint32_t mavlink_msg_named_value_float_get_time_boot_ms(const mavl */ static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_message_t* msg, char *name) { - return MAVLINK_MSG_RETURN_char_array(msg, name, 10, 8); + return _MAV_RETURN_char_array(msg, name, 10, 8); } /** @@ -138,7 +161,7 @@ static inline uint16_t mavlink_msg_named_value_float_get_name(const mavlink_mess */ static inline float mavlink_msg_named_value_float_get_value(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -154,6 +177,6 @@ static inline void mavlink_msg_named_value_float_decode(const mavlink_message_t* named_value_float->value = mavlink_msg_named_value_float_get_value(msg); mavlink_msg_named_value_float_get_name(msg, named_value_float->name); #else - memcpy(named_value_float, MAVLINK_PAYLOAD(msg), 18); + memcpy(named_value_float, _MAV_PAYLOAD(msg), 18); #endif } 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 36f6ec76a..96d494486 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -38,12 +38,21 @@ 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, uint32_t time_boot_ms, const char *name, int32_t value) { - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int32_t_by_index(msg, 4, value); // Signed integer value - put_char_array_by_index(msg, 8, name, 10); // Name of the debug variable +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; return mavlink_finalize_message(msg, system_id, component_id, 18, 44); } @@ -62,12 +71,21 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint32_t time_boot_ms,const char *name,int32_t value) { - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int32_t_by_index(msg, 4, value); // Signed integer value - put_char_array_by_index(msg, 8, name, 10); // Name of the debug variable +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 44); } @@ -96,14 +114,19 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *name, int32_t value) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int32_t_by_index(msg, 4, value); // Signed integer value - put_char_array_by_index(msg, 8, name, 10); // Name of the debug variable - - mavlink_finalize_message_chan_send(msg, chan, 18, 44); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, value); + _mav_put_char_array(buf, 8, name, 10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, buf, 18, 44); +#else + mavlink_named_value_int_t packet; + packet.time_boot_ms = time_boot_ms; + packet.value = value; + memcpy(packet.name, name, sizeof(char)*10); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAMED_VALUE_INT, (const char *)&packet, 18, 44); +#endif } #endif @@ -118,7 +141,7 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, uint */ static inline uint32_t mavlink_msg_named_value_int_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -128,7 +151,7 @@ static inline uint32_t mavlink_msg_named_value_int_get_time_boot_ms(const mavlin */ static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_message_t* msg, char *name) { - return MAVLINK_MSG_RETURN_char_array(msg, name, 10, 8); + return _MAV_RETURN_char_array(msg, name, 10, 8); } /** @@ -138,7 +161,7 @@ static inline uint16_t mavlink_msg_named_value_int_get_name(const mavlink_messag */ static inline int32_t mavlink_msg_named_value_int_get_value(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -154,6 +177,6 @@ static inline void mavlink_msg_named_value_int_decode(const mavlink_message_t* m named_value_int->value = mavlink_msg_named_value_int_get_value(msg); mavlink_msg_named_value_int_get_name(msg, named_value_int->name); #else - memcpy(named_value_int, MAVLINK_PAYLOAD(msg), 18); + memcpy(named_value_int, _MAV_PAYLOAD(msg), 18); #endif } 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 6f8db19f0..b32aeab39 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -53,17 +53,33 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); + + memcpy(_MAV_PAYLOAD(msg), buf, 26); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; - put_float_by_index(msg, 0, nav_roll); // Current desired roll in degrees - put_float_by_index(msg, 4, nav_pitch); // Current desired pitch in degrees - put_float_by_index(msg, 8, alt_error); // Current altitude error in meters - put_float_by_index(msg, 12, aspd_error); // Current airspeed error in meters/second - put_float_by_index(msg, 16, xtrack_error); // Current crosstrack error on x-y plane in meters - put_int16_t_by_index(msg, 20, nav_bearing); // Current desired heading in degrees - put_int16_t_by_index(msg, 22, target_bearing); // Bearing to current waypoint/target in degrees - put_uint16_t_by_index(msg, 24, wp_dist); // Distance to active waypoint in meters + memcpy(_MAV_PAYLOAD(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; return mavlink_finalize_message(msg, system_id, component_id, 26, 183); } @@ -87,17 +103,33 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste 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) { - msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); - put_float_by_index(msg, 0, nav_roll); // Current desired roll in degrees - put_float_by_index(msg, 4, nav_pitch); // Current desired pitch in degrees - put_float_by_index(msg, 8, alt_error); // Current altitude error in meters - put_float_by_index(msg, 12, aspd_error); // Current airspeed error in meters/second - put_float_by_index(msg, 16, xtrack_error); // Current crosstrack error on x-y plane in meters - put_int16_t_by_index(msg, 20, nav_bearing); // Current desired heading in degrees - put_int16_t_by_index(msg, 22, target_bearing); // Bearing to current waypoint/target in degrees - put_uint16_t_by_index(msg, 24, wp_dist); // Distance to active waypoint in meters + memcpy(_MAV_PAYLOAD(msg), buf, 26); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; + memcpy(_MAV_PAYLOAD(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 183); } @@ -131,19 +163,31 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { - MAVLINK_ALIGNED_MESSAGE(msg, 26); - msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, nav_roll); + _mav_put_float(buf, 4, nav_pitch); + _mav_put_float(buf, 8, alt_error); + _mav_put_float(buf, 12, aspd_error); + _mav_put_float(buf, 16, xtrack_error); + _mav_put_int16_t(buf, 20, nav_bearing); + _mav_put_int16_t(buf, 22, target_bearing); + _mav_put_uint16_t(buf, 24, wp_dist); - put_float_by_index(msg, 0, nav_roll); // Current desired roll in degrees - put_float_by_index(msg, 4, nav_pitch); // Current desired pitch in degrees - put_float_by_index(msg, 8, alt_error); // Current altitude error in meters - put_float_by_index(msg, 12, aspd_error); // Current airspeed error in meters/second - put_float_by_index(msg, 16, xtrack_error); // Current crosstrack error on x-y plane in meters - put_int16_t_by_index(msg, 20, nav_bearing); // Current desired heading in degrees - put_int16_t_by_index(msg, 22, target_bearing); // Bearing to current waypoint/target in degrees - put_uint16_t_by_index(msg, 24, wp_dist); // Distance to active waypoint in meters + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, buf, 26, 183); +#else + mavlink_nav_controller_output_t packet; + packet.nav_roll = nav_roll; + packet.nav_pitch = nav_pitch; + packet.alt_error = alt_error; + packet.aspd_error = aspd_error; + packet.xtrack_error = xtrack_error; + packet.nav_bearing = nav_bearing; + packet.target_bearing = target_bearing; + packet.wp_dist = wp_dist; - mavlink_finalize_message_chan_send(msg, chan, 26, 183); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, (const char *)&packet, 26, 183); +#endif } #endif @@ -158,7 +202,7 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan */ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -168,7 +212,7 @@ static inline float mavlink_msg_nav_controller_output_get_nav_roll(const mavlink */ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -178,7 +222,7 @@ static inline float mavlink_msg_nav_controller_output_get_nav_pitch(const mavlin */ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 20); + return _MAV_RETURN_int16_t(msg, 20); } /** @@ -188,7 +232,7 @@ static inline int16_t mavlink_msg_nav_controller_output_get_nav_bearing(const ma */ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 22); + return _MAV_RETURN_int16_t(msg, 22); } /** @@ -198,7 +242,7 @@ static inline int16_t mavlink_msg_nav_controller_output_get_target_bearing(const */ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 24); } /** @@ -208,7 +252,7 @@ static inline uint16_t mavlink_msg_nav_controller_output_get_wp_dist(const mavli */ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -218,7 +262,7 @@ static inline float mavlink_msg_nav_controller_output_get_alt_error(const mavlin */ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -228,7 +272,7 @@ static inline float mavlink_msg_nav_controller_output_get_aspd_error(const mavli */ static inline float mavlink_msg_nav_controller_output_get_xtrack_error(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -249,6 +293,6 @@ static inline void mavlink_msg_nav_controller_output_decode(const mavlink_messag 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); #else - memcpy(nav_controller_output, MAVLINK_PAYLOAD(msg), 26); + memcpy(nav_controller_output, _MAV_PAYLOAD(msg), 26); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h index 9b067b29d..5395376fc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h @@ -47,15 +47,29 @@ typedef struct __mavlink_optical_flow_t static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) { - msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (UNIX) - put_float_by_index(msg, 8, ground_distance); // Ground distance in meters - put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction - put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction - put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID - put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, ground_distance); + _mav_put_int16_t(buf, 12, flow_x); + _mav_put_int16_t(buf, 14, flow_y); + _mav_put_uint8_t(buf, 16, sensor_id); + _mav_put_uint8_t(buf, 17, quality); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; return mavlink_finalize_message(msg, system_id, component_id, 18, 19); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance) { - msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (UNIX) - put_float_by_index(msg, 8, ground_distance); // Ground distance in meters - put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction - put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction - put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID - put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, ground_distance); + _mav_put_int16_t(buf, 12, flow_x); + _mav_put_int16_t(buf, 14, flow_y); + _mav_put_uint8_t(buf, 16, sensor_id); + _mav_put_uint8_t(buf, 17, quality); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 19); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (UNIX) - put_float_by_index(msg, 8, ground_distance); // Ground distance in meters - put_int16_t_by_index(msg, 12, flow_x); // Flow in pixels in x-sensor direction - put_int16_t_by_index(msg, 14, flow_y); // Flow in pixels in y-sensor direction - put_uint8_t_by_index(msg, 16, sensor_id); // Sensor ID - put_uint8_t_by_index(msg, 17, quality); // Optical flow quality / confidence. 0: bad, 255: maximum quality - - mavlink_finalize_message_chan_send(msg, chan, 18, 19); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, ground_distance); + _mav_put_int16_t(buf, 12, flow_x); + _mav_put_int16_t(buf, 14, flow_y); + _mav_put_uint8_t(buf, 16, sensor_id); + _mav_put_uint8_t(buf, 17, quality); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, 18, 19); +#else + mavlink_optical_flow_t packet; + packet.time_usec = time_usec; + packet.ground_distance = ground_distance; + packet.flow_x = flow_x; + packet.flow_y = flow_y; + packet.sensor_id = sensor_id; + packet.quality = quality; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, 18, 19); +#endif } #endif @@ -142,7 +180,7 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -152,7 +190,7 @@ static inline uint64_t mavlink_msg_optical_flow_get_time_usec(const mavlink_mess */ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -162,7 +200,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_messa */ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -172,7 +210,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_ */ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -182,7 +220,7 @@ static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_ */ static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -192,7 +230,7 @@ static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message */ static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); #else - memcpy(optical_flow, MAVLINK_PAYLOAD(msg), 18); + memcpy(optical_flow, _MAV_PAYLOAD(msg), 18); #endif } 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 dd55dc90d..a3a7dfa15 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -35,11 +35,21 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; return mavlink_finalize_message(msg, system_id, component_id, 2, 159); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i mavlink_message_t* msg, uint8_t target_system,uint8_t target_component) { - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 159); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - MAVLINK_ALIGNED_MESSAGE(msg, 2); - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, buf, 2, 159); +#else + mavlink_param_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 2, 159); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, (const char *)&packet, 2, 159); +#endif } #endif @@ -110,7 +136,7 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +146,7 @@ static inline uint8_t mavlink_msg_param_request_list_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_list_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_param_request_list_decode(const mavlink_message_t param_request_list->target_system = mavlink_msg_param_request_list_get_target_system(msg); param_request_list->target_component = mavlink_msg_param_request_list_get_target_component(msg); #else - memcpy(param_request_list, MAVLINK_PAYLOAD(msg), 2); + memcpy(param_request_list, _MAV_PAYLOAD(msg), 2); #endif } 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 5b0e39f8b..7c0e8cbbd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -41,13 +41,23 @@ typedef struct __mavlink_param_request_read_t 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) { - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - - put_int16_t_by_index(msg, 0, param_index); // Parameter index. Send -1 to use the param ID field as identifier - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID - put_char_array_by_index(msg, 4, param_id, 16); // Onboard parameter id +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; return mavlink_finalize_message(msg, system_id, component_id, 20, 214); } @@ -67,13 +77,23 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index) { - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - - put_int16_t_by_index(msg, 0, param_index); // Parameter index. Send -1 to use the param ID field as identifier - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID - put_char_array_by_index(msg, 4, param_id, 16); // Onboard parameter id +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 214); } @@ -103,15 +123,21 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, 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_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - - put_int16_t_by_index(msg, 0, param_index); // Parameter index. Send -1 to use the param ID field as identifier - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID - put_char_array_by_index(msg, 4, param_id, 16); // Onboard parameter id - - mavlink_finalize_message_chan_send(msg, chan, 20, 214); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, buf, 20, 214); +#else + mavlink_param_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(packet.param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ, (const char *)&packet, 20, 214); +#endif } #endif @@ -126,7 +152,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -136,7 +162,7 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_system(const mav */ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -146,7 +172,7 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const */ static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char *param_id) { - return MAVLINK_MSG_RETURN_char_array(msg, param_id, 16, 4); + return _MAV_RETURN_char_array(msg, param_id, 16, 4); } /** @@ -156,7 +182,7 @@ static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink */ static inline int16_t mavlink_msg_param_request_read_get_param_index(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 0); + return _MAV_RETURN_int16_t(msg, 0); } /** @@ -173,6 +199,6 @@ static inline void mavlink_msg_param_request_read_decode(const mavlink_message_t 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); #else - memcpy(param_request_read, MAVLINK_PAYLOAD(msg), 20); + memcpy(param_request_read, _MAV_PAYLOAD(msg), 20); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index f54d1f07e..6b9db4b9e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -44,14 +44,25 @@ typedef struct __mavlink_param_set_t 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, uint8_t param_type) { - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - - put_float_by_index(msg, 0, param_value); // Onboard parameter value - put_uint8_t_by_index(msg, 4, target_system); // System ID - put_uint8_t_by_index(msg, 5, target_component); // Component ID - put_char_array_by_index(msg, 6, param_id, 16); // Onboard parameter id - put_uint8_t_by_index(msg, 22, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[23]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + memcpy(_MAV_PAYLOAD(msg), buf, 23); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD(msg), &packet, 23); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; return mavlink_finalize_message(msg, system_id, component_id, 23, 168); } @@ -72,14 +83,25 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,const char *param_id,float param_value,uint8_t param_type) { - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - - put_float_by_index(msg, 0, param_value); // Onboard parameter value - put_uint8_t_by_index(msg, 4, target_system); // System ID - put_uint8_t_by_index(msg, 5, target_component); // Component ID - put_char_array_by_index(msg, 6, param_id, 16); // Onboard parameter id - put_uint8_t_by_index(msg, 22, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[23]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + memcpy(_MAV_PAYLOAD(msg), buf, 23); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD(msg), &packet, 23); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_SET; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23, 168); } @@ -110,16 +132,23 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c 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, uint8_t param_type) { - MAVLINK_ALIGNED_MESSAGE(msg, 23); - msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - - put_float_by_index(msg, 0, param_value); // Onboard parameter value - put_uint8_t_by_index(msg, 4, target_system); // System ID - put_uint8_t_by_index(msg, 5, target_component); // Component ID - put_char_array_by_index(msg, 6, param_id, 16); // Onboard parameter id - put_uint8_t_by_index(msg, 22, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - - mavlink_finalize_message_chan_send(msg, chan, 23, 168); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[23]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 22, param_type); + _mav_put_char_array(buf, 6, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, buf, 23, 168); +#else + mavlink_param_set_t packet; + packet.param_value = param_value; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + memcpy(packet.param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_SET, (const char *)&packet, 23, 168); +#endif } #endif @@ -134,7 +163,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta */ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -144,7 +173,7 @@ static inline uint8_t mavlink_msg_param_set_get_target_system(const mavlink_mess */ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -154,7 +183,7 @@ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_m */ static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char *param_id) { - return MAVLINK_MSG_RETURN_char_array(msg, param_id, 16, 6); + return _MAV_RETURN_char_array(msg, param_id, 16, 6); } /** @@ -164,7 +193,7 @@ static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_ */ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -174,7 +203,7 @@ static inline float mavlink_msg_param_set_get_param_value(const mavlink_message_ */ static inline uint8_t mavlink_msg_param_set_get_param_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 22); + return _MAV_RETURN_uint8_t(msg, 22); } /** @@ -192,6 +221,6 @@ static inline void mavlink_msg_param_set_decode(const mavlink_message_t* msg, ma mavlink_msg_param_set_get_param_id(msg, param_set->param_id); param_set->param_type = mavlink_msg_param_set_get_param_type(msg); #else - memcpy(param_set, MAVLINK_PAYLOAD(msg), 23); + memcpy(param_set, _MAV_PAYLOAD(msg), 23); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index d08ee44c1..bad89c1e6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -44,14 +44,25 @@ typedef struct __mavlink_param_value_t 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, uint8_t param_type, uint16_t param_count, uint16_t param_index) { - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - - put_float_by_index(msg, 0, param_value); // Onboard parameter value - put_uint16_t_by_index(msg, 4, param_count); // Total number of onboard parameters - put_uint16_t_by_index(msg, 6, param_index); // Index of this onboard parameter - put_char_array_by_index(msg, 8, param_id, 16); // Onboard parameter id - put_uint8_t_by_index(msg, 24, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + memcpy(_MAV_PAYLOAD(msg), buf, 25); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD(msg), &packet, 25); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; return mavlink_finalize_message(msg, system_id, component_id, 25, 220); } @@ -72,14 +83,25 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, const char *param_id,float param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) { - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - - put_float_by_index(msg, 0, param_value); // Onboard parameter value - put_uint16_t_by_index(msg, 4, param_count); // Total number of onboard parameters - put_uint16_t_by_index(msg, 6, param_index); // Index of this onboard parameter - put_char_array_by_index(msg, 8, param_id, 16); // Onboard parameter id - put_uint8_t_by_index(msg, 24, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + memcpy(_MAV_PAYLOAD(msg), buf, 25); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD(msg), &packet, 25); +#endif + msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 220); } @@ -110,16 +132,23 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char *param_id, float param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) { - MAVLINK_ALIGNED_MESSAGE(msg, 25); - msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - - put_float_by_index(msg, 0, param_value); // Onboard parameter value - put_uint16_t_by_index(msg, 4, param_count); // Total number of onboard parameters - put_uint16_t_by_index(msg, 6, param_index); // Index of this onboard parameter - put_char_array_by_index(msg, 8, param_id, 16); // Onboard parameter id - put_uint8_t_by_index(msg, 24, param_type); // Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - - mavlink_finalize_message_chan_send(msg, chan, 25, 220); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, param_value); + _mav_put_uint16_t(buf, 4, param_count); + _mav_put_uint16_t(buf, 6, param_index); + _mav_put_uint8_t(buf, 24, param_type); + _mav_put_char_array(buf, 8, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, buf, 25, 220); +#else + mavlink_param_value_t packet; + packet.param_value = param_value; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + memcpy(packet.param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet, 25, 220); +#endif } #endif @@ -134,7 +163,7 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch */ static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char *param_id) { - return MAVLINK_MSG_RETURN_char_array(msg, param_id, 16, 8); + return _MAV_RETURN_char_array(msg, param_id, 16, 8); } /** @@ -144,7 +173,7 @@ static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_messag */ static inline float mavlink_msg_param_value_get_param_value(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -154,7 +183,7 @@ static inline float mavlink_msg_param_value_get_param_value(const mavlink_messag */ static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 24); + return _MAV_RETURN_uint8_t(msg, 24); } /** @@ -164,7 +193,7 @@ static inline uint8_t mavlink_msg_param_value_get_param_type(const mavlink_messa */ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -174,7 +203,7 @@ static inline uint16_t mavlink_msg_param_value_get_param_count(const mavlink_mes */ static inline uint16_t mavlink_msg_param_value_get_param_index(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -192,6 +221,6 @@ static inline void mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_msg_param_value_get_param_id(msg, param_value->param_id); param_value->param_type = mavlink_msg_param_value_get_param_type(msg); #else - memcpy(param_value, MAVLINK_PAYLOAD(msg), 25); + memcpy(param_value, _MAV_PAYLOAD(msg), 25); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index a3091a34f..a1d185226 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -41,13 +41,25 @@ 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, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) { - msg->msgid = MAVLINK_MSG_ID_PING; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint64_t_by_index(msg, 0, time_usec); // Unix timestamp in microseconds - put_uint32_t_by_index(msg, 8, seq); // PING sequence - put_uint8_t_by_index(msg, 12, target_system); // 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - put_uint8_t_by_index(msg, 13, target_component); // 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_PING; return mavlink_finalize_message(msg, system_id, component_id, 14, 237); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com mavlink_message_t* msg, uint64_t time_usec,uint32_t seq,uint8_t target_system,uint8_t target_component) { - msg->msgid = MAVLINK_MSG_ID_PING; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint64_t_by_index(msg, 0, time_usec); // Unix timestamp in microseconds - put_uint32_t_by_index(msg, 8, seq); // PING sequence - put_uint8_t_by_index(msg, 12, target_system); // 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - put_uint8_t_by_index(msg, 13, target_component); // 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_PING; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 237); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t seq, uint8_t target_system, uint8_t target_component) { - MAVLINK_ALIGNED_MESSAGE(msg, 14); - msg->msgid = MAVLINK_MSG_ID_PING; - - put_uint64_t_by_index(msg, 0, time_usec); // Unix timestamp in microseconds - put_uint32_t_by_index(msg, 8, seq); // PING sequence - put_uint8_t_by_index(msg, 12, target_system); // 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - put_uint8_t_by_index(msg, 13, target_component); // 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint8_t(buf, 13, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, buf, 14, 237); +#else + mavlink_ping_t packet; + packet.time_usec = time_usec; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 14, 237); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PING, (const char *)&packet, 14, 237); +#endif } #endif @@ -126,7 +158,7 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint64_t time_u */ static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -136,7 +168,7 @@ static inline uint64_t mavlink_msg_ping_get_time_usec(const mavlink_message_t* m */ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 8); + return _MAV_RETURN_uint32_t(msg, 8); } /** @@ -146,7 +178,7 @@ static inline uint32_t mavlink_msg_ping_get_seq(const mavlink_message_t* msg) */ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 12); + return _MAV_RETURN_uint8_t(msg, 12); } /** @@ -156,7 +188,7 @@ static inline uint8_t mavlink_msg_ping_get_target_system(const mavlink_message_t */ static inline uint8_t mavlink_msg_ping_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 13); + return _MAV_RETURN_uint8_t(msg, 13); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_ping_decode(const mavlink_message_t* msg, mavlink ping->target_system = mavlink_msg_ping_get_target_system(msg); ping->target_component = mavlink_msg_ping_get_target_component(msg); #else - memcpy(ping, MAVLINK_PAYLOAD(msg), 14); + memcpy(ping, _MAV_PAYLOAD(msg), 14); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h new file mode 100644 index 000000000..b411dee22 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -0,0 +1,210 @@ +// MESSAGE POSITION_TARGET PACKING + +#define MAVLINK_MSG_ID_POSITION_TARGET 63 + +typedef struct __mavlink_position_target_t +{ + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< yaw orientation in radians, 0 = NORTH +} mavlink_position_target_t; + +#define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 +#define MAVLINK_MSG_ID_63_LEN 16 + + + +#define MAVLINK_MESSAGE_INFO_POSITION_TARGET { \ + "POSITION_TARGET", \ + 4, \ + { { "x", MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_position_target_t, x) }, \ + { "y", MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_position_target_t, y) }, \ + { "z", MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_position_target_t, z) }, \ + { "yaw", MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_position_target_t, yaw) }, \ + } \ +} + + +/** + * @brief Pack a position_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param x x position + * @param y y position + * @param z z position + * @param yaw yaw orientation in radians, 0 = NORTH + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; + return mavlink_finalize_message(msg, system_id, component_id, 16, 126); +} + +/** + * @brief Pack a position_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param x x position + * @param y y position + * @param z z position + * @param yaw yaw orientation in radians, 0 = NORTH + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float x,float y,float z,float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 126); +} + +/** + * @brief Encode a position_target struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param position_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_position_target_t* position_target) +{ + return mavlink_msg_position_target_pack(system_id, component_id, msg, position_target->x, position_target->y, position_target->z, position_target->yaw); +} + +/** + * @brief Send a position_target message + * @param chan MAVLink channel to send the message + * + * @param x x position + * @param y y position + * @param z z position + * @param yaw yaw orientation in radians, 0 = NORTH + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET, buf, 16, 126); +#else + mavlink_position_target_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_TARGET, (const char *)&packet, 16, 126); +#endif +} + +#endif + +// MESSAGE POSITION_TARGET UNPACKING + + +/** + * @brief Get field x from position_target message + * + * @return x position + */ +static inline float mavlink_msg_position_target_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field y from position_target message + * + * @return y position + */ +static inline float mavlink_msg_position_target_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field z from position_target message + * + * @return z position + */ +static inline float mavlink_msg_position_target_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yaw from position_target message + * + * @return yaw orientation in radians, 0 = NORTH + */ +static inline float mavlink_msg_position_target_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Decode a position_target message into a struct + * + * @param msg The message to decode + * @param position_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_position_target_decode(const mavlink_message_t* msg, mavlink_position_target_t* position_target) +{ +#if MAVLINK_NEED_BYTE_SWAP + position_target->x = mavlink_msg_position_target_get_x(msg); + position_target->y = mavlink_msg_position_target_get_y(msg); + position_target->z = mavlink_msg_position_target_get_z(msg); + position_target->yaw = mavlink_msg_position_target_get_yaw(msg); +#else + memcpy(position_target, _MAV_PAYLOAD(msg), 16); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index d14875a2c..68dad1de7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -59,19 +59,37 @@ 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 time_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) { - msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, xacc); // X acceleration (raw) - put_int16_t_by_index(msg, 10, yacc); // Y acceleration (raw) - put_int16_t_by_index(msg, 12, zacc); // Z acceleration (raw) - put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (raw) - put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (raw) - put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (raw) - put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (raw) - put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (raw) - put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (raw) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD(msg), buf, 26); +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; return mavlink_finalize_message(msg, system_id, component_id, 26, 144); } @@ -97,19 +115,37 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, uint64_t time_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) { - msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, xacc); // X acceleration (raw) - put_int16_t_by_index(msg, 10, yacc); // Y acceleration (raw) - put_int16_t_by_index(msg, 12, zacc); // Z acceleration (raw) - put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (raw) - put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (raw) - put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (raw) - put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (raw) - put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (raw) - put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (raw) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + memcpy(_MAV_PAYLOAD(msg), buf, 26); +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 144); } @@ -145,21 +181,35 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_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_ALIGNED_MESSAGE(msg, 26); - msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, xacc); // X acceleration (raw) - put_int16_t_by_index(msg, 10, yacc); // Y acceleration (raw) - put_int16_t_by_index(msg, 12, zacc); // Z acceleration (raw) - put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (raw) - put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (raw) - put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (raw) - put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (raw) - put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (raw) - put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (raw) - - mavlink_finalize_message_chan_send(msg, chan, 26, 144); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, xacc); + _mav_put_int16_t(buf, 10, yacc); + _mav_put_int16_t(buf, 12, zacc); + _mav_put_int16_t(buf, 14, xgyro); + _mav_put_int16_t(buf, 16, ygyro); + _mav_put_int16_t(buf, 18, zgyro); + _mav_put_int16_t(buf, 20, xmag); + _mav_put_int16_t(buf, 22, ymag); + _mav_put_int16_t(buf, 24, zmag); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, 26, 144); +#else + mavlink_raw_imu_t packet; + packet.time_usec = time_usec; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, 26, 144); +#endif } #endif @@ -174,7 +224,7 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim */ static inline uint64_t mavlink_msg_raw_imu_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -184,7 +234,7 @@ static inline uint64_t mavlink_msg_raw_imu_get_time_usec(const mavlink_message_t */ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 8); + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -194,7 +244,7 @@ static inline int16_t mavlink_msg_raw_imu_get_xacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 10); + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -204,7 +254,7 @@ static inline int16_t mavlink_msg_raw_imu_get_yacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -214,7 +264,7 @@ static inline int16_t mavlink_msg_raw_imu_get_zacc(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -224,7 +274,7 @@ static inline int16_t mavlink_msg_raw_imu_get_xgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -234,7 +284,7 @@ static inline int16_t mavlink_msg_raw_imu_get_ygyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 18); + return _MAV_RETURN_int16_t(msg, 18); } /** @@ -244,7 +294,7 @@ static inline int16_t mavlink_msg_raw_imu_get_zgyro(const mavlink_message_t* msg */ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 20); + return _MAV_RETURN_int16_t(msg, 20); } /** @@ -254,7 +304,7 @@ static inline int16_t mavlink_msg_raw_imu_get_xmag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 22); + return _MAV_RETURN_int16_t(msg, 22); } /** @@ -264,7 +314,7 @@ static inline int16_t mavlink_msg_raw_imu_get_ymag(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 24); + return _MAV_RETURN_int16_t(msg, 24); } /** @@ -287,6 +337,6 @@ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavl raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); #else - memcpy(raw_imu, MAVLINK_PAYLOAD(msg), 26); + memcpy(raw_imu, _MAV_PAYLOAD(msg), 26); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index f947d3ca8..229c8f19f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -44,14 +44,27 @@ 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 time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, press_abs); // Absolute pressure (raw) - put_int16_t_by_index(msg, 10, press_diff1); // Differential pressure 1 (raw) - put_int16_t_by_index(msg, 12, press_diff2); // Differential pressure 2 (raw) - put_int16_t_by_index(msg, 14, temperature); // Raw Temperature measurement (raw) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; return mavlink_finalize_message(msg, system_id, component_id, 16, 67); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint64_t time_usec,int16_t press_abs,int16_t press_diff1,int16_t press_diff2,int16_t temperature) { - msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, press_abs); // Absolute pressure (raw) - put_int16_t_by_index(msg, 10, press_diff1); // Differential pressure 1 (raw) - put_int16_t_by_index(msg, 12, press_diff2); // Differential pressure 2 (raw) - put_int16_t_by_index(msg, 14, temperature); // Raw Temperature measurement (raw) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 67); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t time_usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { - MAVLINK_ALIGNED_MESSAGE(msg, 16); - msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - - put_uint64_t_by_index(msg, 0, time_usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, press_abs); // Absolute pressure (raw) - put_int16_t_by_index(msg, 10, press_diff1); // Differential pressure 1 (raw) - put_int16_t_by_index(msg, 12, press_diff2); // Differential pressure 2 (raw) - put_int16_t_by_index(msg, 14, temperature); // Raw Temperature measurement (raw) - - mavlink_finalize_message_chan_send(msg, chan, 16, 67); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_int16_t(buf, 8, press_abs); + _mav_put_int16_t(buf, 10, press_diff1); + _mav_put_int16_t(buf, 12, press_diff2); + _mav_put_int16_t(buf, 14, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, buf, 16, 67); +#else + mavlink_raw_pressure_t packet; + packet.time_usec = time_usec; + packet.press_abs = press_abs; + packet.press_diff1 = press_diff1; + packet.press_diff2 = press_diff2; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_PRESSURE, (const char *)&packet, 16, 67); +#endif } #endif @@ -134,7 +169,7 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ */ static inline uint64_t mavlink_msg_raw_pressure_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -144,7 +179,7 @@ static inline uint64_t mavlink_msg_raw_pressure_get_time_usec(const mavlink_mess */ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 8); + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -154,7 +189,7 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_abs(const mavlink_messa */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 10); + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -164,7 +199,7 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff1(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -174,7 +209,7 @@ static inline int16_t mavlink_msg_raw_pressure_get_press_diff2(const mavlink_mes */ static inline int16_t mavlink_msg_raw_pressure_get_temperature(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_raw_pressure_decode(const mavlink_message_t* msg, raw_pressure->press_diff2 = mavlink_msg_raw_pressure_get_press_diff2(msg); raw_pressure->temperature = mavlink_msg_raw_pressure_get_temperature(msg); #else - memcpy(raw_pressure, MAVLINK_PAYLOAD(msg), 16); + memcpy(raw_pressure, _MAV_PAYLOAD(msg), 16); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h index 9b99342c1..c18d834e2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -59,19 +59,37 @@ typedef struct __mavlink_rc_channels_override_t static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) { - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - - put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; return mavlink_finalize_message(msg, system_id, component_id, 18, 124); } @@ -97,19 +115,37 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) { - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - - put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 124); } @@ -145,21 +181,35 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - - put_uint16_t_by_index(msg, 0, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 2, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 4, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 6, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 8, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 10, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 12, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 14, chan8_raw); // RC channel 8 value, in microseconds - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 18, 124); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_uint16_t(buf, 0, chan1_raw); + _mav_put_uint16_t(buf, 2, chan2_raw); + _mav_put_uint16_t(buf, 4, chan3_raw); + _mav_put_uint16_t(buf, 6, chan4_raw); + _mav_put_uint16_t(buf, 8, chan5_raw); + _mav_put_uint16_t(buf, 10, chan6_raw); + _mav_put_uint16_t(buf, 12, chan7_raw); + _mav_put_uint16_t(buf, 14, chan8_raw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, 18, 124); +#else + mavlink_rc_channels_override_t packet; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, 18, 124); +#endif } #endif @@ -174,7 +224,7 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -184,7 +234,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const m */ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -194,7 +244,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -204,7 +254,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 2); + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -214,7 +264,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -224,7 +274,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -234,7 +284,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -244,7 +294,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -254,7 +304,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -264,7 +314,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -287,6 +337,6 @@ static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); #else - memcpy(rc_channels_override, MAVLINK_PAYLOAD(msg), 18); + memcpy(rc_channels_override, _MAV_PAYLOAD(msg), 18); #endif } 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 d5b6411b3..23c54ff3c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -62,20 +62,39 @@ 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, uint32_t time_boot_ms, uint8_t port, 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) { - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_uint16_t_by_index(msg, 4, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 6, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 8, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 10, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 12, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 14, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 16, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 18, chan8_raw); // RC channel 8 value, in microseconds - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - put_uint8_t_by_index(msg, 21, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 22); +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 22); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; return mavlink_finalize_message(msg, system_id, component_id, 22, 244); } @@ -102,20 +121,39 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint32_t time_boot_ms,uint8_t port,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) { - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_uint16_t_by_index(msg, 4, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 6, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 8, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 10, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 12, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 14, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 16, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 18, chan8_raw); // RC channel 8 value, in microseconds - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - put_uint8_t_by_index(msg, 21, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 22); +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 22); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 244); } @@ -152,22 +190,37 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, 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_ALIGNED_MESSAGE(msg, 22); - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_uint16_t_by_index(msg, 4, chan1_raw); // RC channel 1 value, in microseconds - put_uint16_t_by_index(msg, 6, chan2_raw); // RC channel 2 value, in microseconds - put_uint16_t_by_index(msg, 8, chan3_raw); // RC channel 3 value, in microseconds - put_uint16_t_by_index(msg, 10, chan4_raw); // RC channel 4 value, in microseconds - put_uint16_t_by_index(msg, 12, chan5_raw); // RC channel 5 value, in microseconds - put_uint16_t_by_index(msg, 14, chan6_raw); // RC channel 6 value, in microseconds - put_uint16_t_by_index(msg, 16, chan7_raw); // RC channel 7 value, in microseconds - put_uint16_t_by_index(msg, 18, chan8_raw); // RC channel 8 value, in microseconds - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - put_uint8_t_by_index(msg, 21, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% - - mavlink_finalize_message_chan_send(msg, chan, 22, 244); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint16_t(buf, 4, chan1_raw); + _mav_put_uint16_t(buf, 6, chan2_raw); + _mav_put_uint16_t(buf, 8, chan3_raw); + _mav_put_uint16_t(buf, 10, chan4_raw); + _mav_put_uint16_t(buf, 12, chan5_raw); + _mav_put_uint16_t(buf, 14, chan6_raw); + _mav_put_uint16_t(buf, 16, chan7_raw); + _mav_put_uint16_t(buf, 18, chan8_raw); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, buf, 22, 244); +#else + mavlink_rc_channels_raw_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_raw = chan1_raw; + packet.chan2_raw = chan2_raw; + packet.chan3_raw = chan3_raw; + packet.chan4_raw = chan4_raw; + packet.chan5_raw = chan5_raw; + packet.chan6_raw = chan6_raw; + packet.chan7_raw = chan7_raw; + packet.chan8_raw = chan8_raw; + packet.port = port; + packet.rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW, (const char *)&packet, 22, 244); +#endif } #endif @@ -182,7 +235,7 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint */ static inline uint32_t mavlink_msg_rc_channels_raw_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -192,7 +245,7 @@ static inline uint32_t mavlink_msg_rc_channels_raw_get_time_boot_ms(const mavlin */ static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -202,7 +255,7 @@ static inline uint8_t mavlink_msg_rc_channels_raw_get_port(const mavlink_message */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -212,7 +265,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan1_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -222,7 +275,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan2_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -232,7 +285,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan3_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -242,7 +295,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan4_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -252,7 +305,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan5_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -262,7 +315,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan6_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -272,7 +325,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan7_raw(const mavlink_m */ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** @@ -282,7 +335,7 @@ static inline uint16_t mavlink_msg_rc_channels_raw_get_chan8_raw(const mavlink_m */ static inline uint8_t mavlink_msg_rc_channels_raw_get_rssi(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 21); + return _MAV_RETURN_uint8_t(msg, 21); } /** @@ -306,6 +359,6 @@ static inline void mavlink_msg_rc_channels_raw_decode(const mavlink_message_t* m rc_channels_raw->port = mavlink_msg_rc_channels_raw_get_port(msg); rc_channels_raw->rssi = mavlink_msg_rc_channels_raw_get_rssi(msg); #else - memcpy(rc_channels_raw, MAVLINK_PAYLOAD(msg), 22); + memcpy(rc_channels_raw, _MAV_PAYLOAD(msg), 22); #endif } 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 766816874..516bc496c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -62,20 +62,39 @@ 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, uint32_t time_boot_ms, uint8_t port, 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) { - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int16_t_by_index(msg, 4, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 6, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 8, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 10, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 12, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 14, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 16, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 18, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - put_uint8_t_by_index(msg, 21, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 22); +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 22); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; return mavlink_finalize_message(msg, system_id, component_id, 22, 237); } @@ -102,20 +121,39 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i mavlink_message_t* msg, uint32_t time_boot_ms,uint8_t port,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) { - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int16_t_by_index(msg, 4, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 6, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 8, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 10, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 12, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 14, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 16, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 18, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - put_uint8_t_by_index(msg, 21, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + memcpy(_MAV_PAYLOAD(msg), buf, 22); +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + + memcpy(_MAV_PAYLOAD(msg), &packet, 22); +#endif + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 237); } @@ -152,22 +190,37 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t port, 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_ALIGNED_MESSAGE(msg, 22); - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int16_t_by_index(msg, 4, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 6, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 8, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 10, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 12, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 14, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 16, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 18, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - put_uint8_t_by_index(msg, 21, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% - - mavlink_finalize_message_chan_send(msg, chan, 22, 237); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, chan1_scaled); + _mav_put_int16_t(buf, 6, chan2_scaled); + _mav_put_int16_t(buf, 8, chan3_scaled); + _mav_put_int16_t(buf, 10, chan4_scaled); + _mav_put_int16_t(buf, 12, chan5_scaled); + _mav_put_int16_t(buf, 14, chan6_scaled); + _mav_put_int16_t(buf, 16, chan7_scaled); + _mav_put_int16_t(buf, 18, chan8_scaled); + _mav_put_uint8_t(buf, 20, port); + _mav_put_uint8_t(buf, 21, rssi); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, buf, 22, 237); +#else + mavlink_rc_channels_scaled_t packet; + packet.time_boot_ms = time_boot_ms; + packet.chan1_scaled = chan1_scaled; + packet.chan2_scaled = chan2_scaled; + packet.chan3_scaled = chan3_scaled; + packet.chan4_scaled = chan4_scaled; + packet.chan5_scaled = chan5_scaled; + packet.chan6_scaled = chan6_scaled; + packet.chan7_scaled = chan7_scaled; + packet.chan8_scaled = chan8_scaled; + packet.port = port; + packet.rssi = rssi; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, (const char *)&packet, 22, 237); +#endif } #endif @@ -182,7 +235,7 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, u */ static inline uint32_t mavlink_msg_rc_channels_scaled_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -192,7 +245,7 @@ static inline uint32_t mavlink_msg_rc_channels_scaled_get_time_boot_ms(const mav */ static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -202,7 +255,7 @@ static inline uint8_t mavlink_msg_rc_channels_scaled_get_port(const mavlink_mess */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 4); + return _MAV_RETURN_int16_t(msg, 4); } /** @@ -212,7 +265,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan1_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 6); + return _MAV_RETURN_int16_t(msg, 6); } /** @@ -222,7 +275,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan2_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 8); + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -232,7 +285,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan3_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 10); + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -242,7 +295,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan4_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -252,7 +305,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan5_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -262,7 +315,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan6_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -272,7 +325,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan7_scaled(const mavl */ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 18); + return _MAV_RETURN_int16_t(msg, 18); } /** @@ -282,7 +335,7 @@ static inline int16_t mavlink_msg_rc_channels_scaled_get_chan8_scaled(const mavl */ static inline uint8_t mavlink_msg_rc_channels_scaled_get_rssi(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 21); + return _MAV_RETURN_uint8_t(msg, 21); } /** @@ -306,6 +359,6 @@ static inline void mavlink_msg_rc_channels_scaled_decode(const mavlink_message_t rc_channels_scaled->port = mavlink_msg_rc_channels_scaled_get_port(msg); rc_channels_scaled->rssi = mavlink_msg_rc_channels_scaled_get_rssi(msg); #else - memcpy(rc_channels_scaled, MAVLINK_PAYLOAD(msg), 22); + memcpy(rc_channels_scaled, _MAV_PAYLOAD(msg), 22); #endif } 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 782f9ad04..18bd09b50 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -44,14 +44,27 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - - put_uint16_t_by_index(msg, 0, req_message_rate); // The requested interval between two messages of this type - put_uint8_t_by_index(msg, 2, target_system); // The target requested to send the message stream. - put_uint8_t_by_index(msg, 3, target_component); // The target requested to send the message stream. - put_uint8_t_by_index(msg, 4, req_stream_id); // The ID of the requested data stream - put_uint8_t_by_index(msg, 5, start_stop); // 1 to start sending, 0 to stop sending. +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; return mavlink_finalize_message(msg, system_id, component_id, 6, 148); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ 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) { - msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - - put_uint16_t_by_index(msg, 0, req_message_rate); // The requested interval between two messages of this type - put_uint8_t_by_index(msg, 2, target_system); // The target requested to send the message stream. - put_uint8_t_by_index(msg, 3, target_component); // The target requested to send the message stream. - put_uint8_t_by_index(msg, 4, req_stream_id); // The ID of the requested data stream - put_uint8_t_by_index(msg, 5, start_stop); // 1 to start sending, 0 to stop sending. +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 148); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { - MAVLINK_ALIGNED_MESSAGE(msg, 6); - msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - - put_uint16_t_by_index(msg, 0, req_message_rate); // The requested interval between two messages of this type - put_uint8_t_by_index(msg, 2, target_system); // The target requested to send the message stream. - put_uint8_t_by_index(msg, 3, target_component); // The target requested to send the message stream. - put_uint8_t_by_index(msg, 4, req_stream_id); // The ID of the requested data stream - put_uint8_t_by_index(msg, 5, start_stop); // 1 to start sending, 0 to stop sending. - - mavlink_finalize_message_chan_send(msg, chan, 6, 148); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint16_t(buf, 0, req_message_rate); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, req_stream_id); + _mav_put_uint8_t(buf, 5, start_stop); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, buf, 6, 148); +#else + mavlink_request_data_stream_t packet; + packet.req_message_rate = req_message_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.req_stream_id = req_stream_id; + packet.start_stop = start_stop; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, (const char *)&packet, 6, 148); +#endif } #endif @@ -134,7 +169,7 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -144,7 +179,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_system(const ma */ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -154,7 +189,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_target_component(const */ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -164,7 +199,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma */ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -174,7 +209,7 @@ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(cons */ static inline uint8_t mavlink_msg_request_data_stream_get_start_stop(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_request_data_stream_decode(const mavlink_message_ request_data_stream->req_stream_id = mavlink_msg_request_data_stream_get_req_stream_id(msg); request_data_stream->start_stop = mavlink_msg_request_data_stream_get_start_stop(msg); #else - memcpy(request_data_stream, MAVLINK_PAYLOAD(msg), 6); + memcpy(request_data_stream, _MAV_PAYLOAD(msg), 6); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h index e4b884cdd..b2c1baebf 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -44,14 +44,27 @@ typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp in milliseconds since system boot - put_float_by_index(msg, 4, roll_speed); // Desired roll angular speed in rad/s - put_float_by_index(msg, 8, pitch_speed); // Desired pitch angular speed in rad/s - put_float_by_index(msg, 12, yaw_speed); // Desired yaw angular speed in rad/s - put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_speed); + _mav_put_float(buf, 8, pitch_speed); + _mav_put_float(buf, 12, yaw_speed); + _mav_put_float(buf, 16, thrust); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; return mavlink_finalize_message(msg, system_id, component_id, 20, 238); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha mavlink_message_t* msg, uint32_t time_boot_ms,float roll_speed,float pitch_speed,float yaw_speed,float thrust) { - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp in milliseconds since system boot - put_float_by_index(msg, 4, roll_speed); // Desired roll angular speed in rad/s - put_float_by_index(msg, 8, pitch_speed); // Desired pitch angular speed in rad/s - put_float_by_index(msg, 12, yaw_speed); // Desired yaw angular speed in rad/s - put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_speed); + _mav_put_float(buf, 8, pitch_speed); + _mav_put_float(buf, 12, yaw_speed); + _mav_put_float(buf, 16, thrust); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 238); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(u static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - MAVLINK_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp in milliseconds since system boot - put_float_by_index(msg, 4, roll_speed); // Desired roll angular speed in rad/s - put_float_by_index(msg, 8, pitch_speed); // Desired pitch angular speed in rad/s - put_float_by_index(msg, 12, yaw_speed); // Desired yaw angular speed in rad/s - put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 - - mavlink_finalize_message_chan_send(msg, chan, 20, 238); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll_speed); + _mav_put_float(buf, 8, pitch_speed); + _mav_put_float(buf, 12, yaw_speed); + _mav_put_float(buf, 16, thrust); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, buf, 20, 238); +#else + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, (const char *)&packet, 20, 238); +#endif } #endif @@ -134,7 +169,7 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink */ static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -144,7 +179,7 @@ static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -154,7 +189,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_sp */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -164,7 +199,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_s */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -174,7 +209,7 @@ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_spe */ static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg); roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg); #else - memcpy(roll_pitch_yaw_speed_thrust_setpoint, MAVLINK_PAYLOAD(msg), 20); + memcpy(roll_pitch_yaw_speed_thrust_setpoint, _MAV_PAYLOAD(msg), 20); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h index 83a42a0b1..2ae32c005 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -44,14 +44,27 @@ typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) { - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp in milliseconds since system boot - put_float_by_index(msg, 4, roll); // Desired roll angle in radians - put_float_by_index(msg, 8, pitch); // Desired pitch angle in radians - put_float_by_index(msg, 12, yaw); // Desired yaw angle in radians - put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; return mavlink_finalize_message(msg, system_id, component_id, 20, 239); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint mavlink_message_t* msg, uint32_t time_boot_ms,float roll,float pitch,float yaw,float thrust) { - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp in milliseconds since system boot - put_float_by_index(msg, 4, roll); // Desired roll angle in radians - put_float_by_index(msg, 8, pitch); // Desired pitch angle in radians - put_float_by_index(msg, 12, yaw); // Desired yaw angle in radians - put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 239); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float thrust) { - MAVLINK_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp in milliseconds since system boot - put_float_by_index(msg, 4, roll); // Desired roll angle in radians - put_float_by_index(msg, 8, pitch); // Desired pitch angle in radians - put_float_by_index(msg, 12, yaw); // Desired yaw angle in radians - put_float_by_index(msg, 16, thrust); // Collective thrust, normalized to 0 .. 1 - - mavlink_finalize_message_chan_send(msg, chan, 20, 239); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, thrust); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, buf, 20, 239); +#else + mavlink_roll_pitch_yaw_thrust_setpoint_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, (const char *)&packet, 20, 239); +#endif } #endif @@ -134,7 +169,7 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_chann */ static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -144,7 +179,7 @@ static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_boot_ */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -154,7 +189,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const ma */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -164,7 +199,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const m */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -174,7 +209,7 @@ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mav */ static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavli roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg); roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg); #else - memcpy(roll_pitch_yaw_thrust_setpoint, MAVLINK_PAYLOAD(msg), 20); + memcpy(roll_pitch_yaw_thrust_setpoint, _MAV_PAYLOAD(msg), 20); #endif } 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 d4838c4ca..fd6d6e9f9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD(msg), buf, 25); +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; - put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 - put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 - put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 - put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 - put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 - put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 - put_uint8_t_by_index(msg, 24, frame); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + memcpy(_MAV_PAYLOAD(msg), &packet, 25); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; return mavlink_finalize_message(msg, system_id, component_id, 25, 3); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ mavlink_message_t* msg, uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) { - msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); - put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 - put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 - put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 - put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 - put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 - put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 - put_uint8_t_by_index(msg, 24, frame); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + memcpy(_MAV_PAYLOAD(msg), buf, 25); +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; + memcpy(_MAV_PAYLOAD(msg), &packet, 25); +#endif + + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 3); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - MAVLINK_ALIGNED_MESSAGE(msg, 25); - msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[25]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, frame); - put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 - put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 - put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 - put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 - put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 - put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 - put_uint8_t_by_index(msg, 24, frame); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, buf, 25, 3); +#else + mavlink_safety_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.frame = frame; - mavlink_finalize_message_chan_send(msg, chan, 25, 3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, (const char *)&packet, 25, 3); +#endif } #endif @@ -150,7 +191,7 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 24); + return _MAV_RETURN_uint8_t(msg, 24); } /** @@ -160,7 +201,7 @@ static inline uint8_t mavlink_msg_safety_allowed_area_get_frame(const mavlink_me */ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -170,7 +211,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p1x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -180,7 +221,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p1y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -190,7 +231,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p1z(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -200,7 +241,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p2x(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -210,7 +251,7 @@ static inline float mavlink_msg_safety_allowed_area_get_p2y(const mavlink_messag */ static inline float mavlink_msg_safety_allowed_area_get_p2z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_safety_allowed_area_decode(const mavlink_message_ safety_allowed_area->p2z = mavlink_msg_safety_allowed_area_get_p2z(msg); safety_allowed_area->frame = mavlink_msg_safety_allowed_area_get_frame(msg); #else - memcpy(safety_allowed_area, MAVLINK_PAYLOAD(msg), 25); + memcpy(safety_allowed_area, _MAV_PAYLOAD(msg), 25); #endif } 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 f04648a4f..58ecd7311 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 @@ -56,18 +56,35 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[27]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + + memcpy(_MAV_PAYLOAD(msg), buf, 27); +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; - put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 - put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 - put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 - put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 - put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 - put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 - put_uint8_t_by_index(msg, 24, target_system); // System ID - put_uint8_t_by_index(msg, 25, target_component); // Component ID - put_uint8_t_by_index(msg, 26, frame); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + memcpy(_MAV_PAYLOAD(msg), &packet, 27); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; return mavlink_finalize_message(msg, system_id, component_id, 27, 15); } @@ -92,18 +109,35 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys 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) { - msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[27]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); + + memcpy(_MAV_PAYLOAD(msg), buf, 27); +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; - put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 - put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 - put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 - put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 - put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 - put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 - put_uint8_t_by_index(msg, 24, target_system); // System ID - put_uint8_t_by_index(msg, 25, target_component); // Component ID - put_uint8_t_by_index(msg, 26, frame); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + memcpy(_MAV_PAYLOAD(msg), &packet, 27); +#endif + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 27, 15); } @@ -138,20 +172,33 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { - MAVLINK_ALIGNED_MESSAGE(msg, 27); - msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[27]; + _mav_put_float(buf, 0, p1x); + _mav_put_float(buf, 4, p1y); + _mav_put_float(buf, 8, p1z); + _mav_put_float(buf, 12, p2x); + _mav_put_float(buf, 16, p2y); + _mav_put_float(buf, 20, p2z); + _mav_put_uint8_t(buf, 24, target_system); + _mav_put_uint8_t(buf, 25, target_component); + _mav_put_uint8_t(buf, 26, frame); - put_float_by_index(msg, 0, p1x); // x position 1 / Latitude 1 - put_float_by_index(msg, 4, p1y); // y position 1 / Longitude 1 - put_float_by_index(msg, 8, p1z); // z position 1 / Altitude 1 - put_float_by_index(msg, 12, p2x); // x position 2 / Latitude 2 - put_float_by_index(msg, 16, p2y); // y position 2 / Longitude 2 - put_float_by_index(msg, 20, p2z); // z position 2 / Altitude 2 - put_uint8_t_by_index(msg, 24, target_system); // System ID - put_uint8_t_by_index(msg, 25, target_component); // Component ID - put_uint8_t_by_index(msg, 26, frame); // Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - - mavlink_finalize_message_chan_send(msg, chan, 27, 15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, buf, 27, 15); +#else + mavlink_safety_set_allowed_area_t packet; + packet.p1x = p1x; + packet.p1y = p1y; + packet.p1z = p1z; + packet.p2x = p2x; + packet.p2y = p2y; + packet.p2z = p2z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, (const char *)&packet, 27, 15); +#endif } #endif @@ -166,7 +213,7 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 24); + return _MAV_RETURN_uint8_t(msg, 24); } /** @@ -176,7 +223,7 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_system(cons */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 25); + return _MAV_RETURN_uint8_t(msg, 25); } /** @@ -186,7 +233,7 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_target_component(c */ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 26); + return _MAV_RETURN_uint8_t(msg, 26); } /** @@ -196,7 +243,7 @@ static inline uint8_t mavlink_msg_safety_set_allowed_area_get_frame(const mavlin */ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -206,7 +253,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -216,7 +263,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -226,7 +273,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p1z(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -236,7 +283,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2x(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -246,7 +293,7 @@ static inline float mavlink_msg_safety_set_allowed_area_get_p2y(const mavlink_me */ static inline float mavlink_msg_safety_set_allowed_area_get_p2z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -268,6 +315,6 @@ static inline void mavlink_msg_safety_set_allowed_area_decode(const mavlink_mess 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); #else - memcpy(safety_set_allowed_area, MAVLINK_PAYLOAD(msg), 27); + memcpy(safety_set_allowed_area, _MAV_PAYLOAD(msg), 27); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index ed4872ed5..27b034fe9 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -59,19 +59,37 @@ 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, uint32_t time_boot_ms, 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) { - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int16_t_by_index(msg, 4, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 6, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 8, zacc); // Z acceleration (mg) - put_int16_t_by_index(msg, 10, xgyro); // Angular speed around X axis (millirad /sec) - put_int16_t_by_index(msg, 12, ygyro); // Angular speed around Y axis (millirad /sec) - put_int16_t_by_index(msg, 14, zgyro); // Angular speed around Z axis (millirad /sec) - put_int16_t_by_index(msg, 16, xmag); // X Magnetic field (milli tesla) - put_int16_t_by_index(msg, 18, ymag); // Y Magnetic field (milli tesla) - put_int16_t_by_index(msg, 20, zmag); // Z Magnetic field (milli tesla) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD(msg), buf, 22); +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD(msg), &packet, 22); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; return mavlink_finalize_message(msg, system_id, component_id, 22, 170); } @@ -97,19 +115,37 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 mavlink_message_t* msg, uint32_t time_boot_ms,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) { - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int16_t_by_index(msg, 4, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 6, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 8, zacc); // Z acceleration (mg) - put_int16_t_by_index(msg, 10, xgyro); // Angular speed around X axis (millirad /sec) - put_int16_t_by_index(msg, 12, ygyro); // Angular speed around Y axis (millirad /sec) - put_int16_t_by_index(msg, 14, zgyro); // Angular speed around Z axis (millirad /sec) - put_int16_t_by_index(msg, 16, xmag); // X Magnetic field (milli tesla) - put_int16_t_by_index(msg, 18, ymag); // Y Magnetic field (milli tesla) - put_int16_t_by_index(msg, 20, zmag); // Z Magnetic field (milli tesla) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + memcpy(_MAV_PAYLOAD(msg), buf, 22); +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + memcpy(_MAV_PAYLOAD(msg), &packet, 22); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 170); } @@ -145,21 +181,35 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, 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_ALIGNED_MESSAGE(msg, 22); - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (milliseconds since system boot) - put_int16_t_by_index(msg, 4, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 6, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 8, zacc); // Z acceleration (mg) - put_int16_t_by_index(msg, 10, xgyro); // Angular speed around X axis (millirad /sec) - put_int16_t_by_index(msg, 12, ygyro); // Angular speed around Y axis (millirad /sec) - put_int16_t_by_index(msg, 14, zgyro); // Angular speed around Z axis (millirad /sec) - put_int16_t_by_index(msg, 16, xmag); // X Magnetic field (milli tesla) - put_int16_t_by_index(msg, 18, ymag); // Y Magnetic field (milli tesla) - put_int16_t_by_index(msg, 20, zmag); // Z Magnetic field (milli tesla) - - mavlink_finalize_message_chan_send(msg, chan, 22, 170); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int16_t(buf, 4, xacc); + _mav_put_int16_t(buf, 6, yacc); + _mav_put_int16_t(buf, 8, zacc); + _mav_put_int16_t(buf, 10, xgyro); + _mav_put_int16_t(buf, 12, ygyro); + _mav_put_int16_t(buf, 14, zgyro); + _mav_put_int16_t(buf, 16, xmag); + _mav_put_int16_t(buf, 18, ymag); + _mav_put_int16_t(buf, 20, zmag); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, 22, 170); +#else + mavlink_scaled_imu_t packet; + packet.time_boot_ms = time_boot_ms; + packet.xacc = xacc; + packet.yacc = yacc; + packet.zacc = zacc; + packet.xgyro = xgyro; + packet.ygyro = ygyro; + packet.zgyro = zgyro; + packet.xmag = xmag; + packet.ymag = ymag; + packet.zmag = zmag; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, 22, 170); +#endif } #endif @@ -174,7 +224,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t */ static inline uint32_t mavlink_msg_scaled_imu_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -184,7 +234,7 @@ static inline uint32_t mavlink_msg_scaled_imu_get_time_boot_ms(const mavlink_mes */ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 4); + return _MAV_RETURN_int16_t(msg, 4); } /** @@ -194,7 +244,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_xacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 6); + return _MAV_RETURN_int16_t(msg, 6); } /** @@ -204,7 +254,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_yacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 8); + return _MAV_RETURN_int16_t(msg, 8); } /** @@ -214,7 +264,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_zacc(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 10); + return _MAV_RETURN_int16_t(msg, 10); } /** @@ -224,7 +274,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_xgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -234,7 +284,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_ygyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -244,7 +294,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_zgyro(const mavlink_message_t* */ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -254,7 +304,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_xmag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 18); + return _MAV_RETURN_int16_t(msg, 18); } /** @@ -264,7 +314,7 @@ static inline int16_t mavlink_msg_scaled_imu_get_ymag(const mavlink_message_t* m */ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 20); + return _MAV_RETURN_int16_t(msg, 20); } /** @@ -287,6 +337,6 @@ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, m scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); #else - memcpy(scaled_imu, MAVLINK_PAYLOAD(msg), 22); + memcpy(scaled_imu, _MAV_PAYLOAD(msg), 22); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 37d36b5be..21818ebf6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -41,13 +41,25 @@ 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, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) { - msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 4, press_abs); // Absolute pressure (hectopascal) - put_float_by_index(msg, 8, press_diff); // Differential pressure 1 (hectopascal) - put_int16_t_by_index(msg, 12, temperature); // Temperature measurement (0.01 degrees celsius) + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; return mavlink_finalize_message(msg, system_id, component_id, 14, 115); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) { - msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 4, press_abs); // Absolute pressure (hectopascal) - put_float_by_index(msg, 8, press_diff); // Differential pressure 1 (hectopascal) - put_int16_t_by_index(msg, 12, temperature); // Temperature measurement (0.01 degrees celsius) + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 115); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) { - MAVLINK_ALIGNED_MESSAGE(msg, 14); - msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - - put_uint32_t_by_index(msg, 0, time_boot_ms); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 4, press_abs); // Absolute pressure (hectopascal) - put_float_by_index(msg, 8, press_diff); // Differential pressure 1 (hectopascal) - put_int16_t_by_index(msg, 12, temperature); // Temperature measurement (0.01 degrees celsius) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, press_abs); + _mav_put_float(buf, 8, press_diff); + _mav_put_int16_t(buf, 12, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, 14, 115); +#else + mavlink_scaled_pressure_t packet; + packet.time_boot_ms = time_boot_ms; + packet.press_abs = press_abs; + packet.press_diff = press_diff; + packet.temperature = temperature; - mavlink_finalize_message_chan_send(msg, chan, 14, 115); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, 14, 115); +#endif } #endif @@ -126,7 +158,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint */ static inline uint32_t mavlink_msg_scaled_pressure_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -136,7 +168,7 @@ static inline uint32_t mavlink_msg_scaled_pressure_get_time_boot_ms(const mavlin */ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -146,7 +178,7 @@ static inline float mavlink_msg_scaled_pressure_get_press_abs(const mavlink_mess */ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -156,7 +188,7 @@ static inline float mavlink_msg_scaled_pressure_get_press_diff(const mavlink_mes */ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* m scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); #else - memcpy(scaled_pressure, MAVLINK_PAYLOAD(msg), 14); + memcpy(scaled_pressure, _MAV_PAYLOAD(msg), 14); #endif } 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 8a9c8618b..67dfc2bb6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -59,19 +59,37 @@ 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, uint32_t time_usec, uint8_t port, 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) { - msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - - put_uint32_t_by_index(msg, 0, time_usec); // Timestamp (since UNIX epoch or microseconds since system boot) - put_uint16_t_by_index(msg, 4, servo1_raw); // Servo output 1 value, in microseconds - put_uint16_t_by_index(msg, 6, servo2_raw); // Servo output 2 value, in microseconds - put_uint16_t_by_index(msg, 8, servo3_raw); // Servo output 3 value, in microseconds - put_uint16_t_by_index(msg, 10, servo4_raw); // Servo output 4 value, in microseconds - put_uint16_t_by_index(msg, 12, servo5_raw); // Servo output 5 value, in microseconds - put_uint16_t_by_index(msg, 14, servo6_raw); // Servo output 6 value, in microseconds - put_uint16_t_by_index(msg, 16, servo7_raw); // Servo output 7 value, in microseconds - put_uint16_t_by_index(msg, 18, servo8_raw); // Servo output 8 value, in microseconds - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + + memcpy(_MAV_PAYLOAD(msg), buf, 21); +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; return mavlink_finalize_message(msg, system_id, component_id, 21, 222); } @@ -97,19 +115,37 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint32_t time_usec,uint8_t port,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) { - msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - - put_uint32_t_by_index(msg, 0, time_usec); // Timestamp (since UNIX epoch or microseconds since system boot) - put_uint16_t_by_index(msg, 4, servo1_raw); // Servo output 1 value, in microseconds - put_uint16_t_by_index(msg, 6, servo2_raw); // Servo output 2 value, in microseconds - put_uint16_t_by_index(msg, 8, servo3_raw); // Servo output 3 value, in microseconds - put_uint16_t_by_index(msg, 10, servo4_raw); // Servo output 4 value, in microseconds - put_uint16_t_by_index(msg, 12, servo5_raw); // Servo output 5 value, in microseconds - put_uint16_t_by_index(msg, 14, servo6_raw); // Servo output 6 value, in microseconds - put_uint16_t_by_index(msg, 16, servo7_raw); // Servo output 7 value, in microseconds - put_uint16_t_by_index(msg, 18, servo8_raw); // Servo output 8 value, in microseconds - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + + memcpy(_MAV_PAYLOAD(msg), buf, 21); +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 222); } @@ -145,21 +181,35 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, 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_ALIGNED_MESSAGE(msg, 21); - msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - - put_uint32_t_by_index(msg, 0, time_usec); // Timestamp (since UNIX epoch or microseconds since system boot) - put_uint16_t_by_index(msg, 4, servo1_raw); // Servo output 1 value, in microseconds - put_uint16_t_by_index(msg, 6, servo2_raw); // Servo output 2 value, in microseconds - put_uint16_t_by_index(msg, 8, servo3_raw); // Servo output 3 value, in microseconds - put_uint16_t_by_index(msg, 10, servo4_raw); // Servo output 4 value, in microseconds - put_uint16_t_by_index(msg, 12, servo5_raw); // Servo output 5 value, in microseconds - put_uint16_t_by_index(msg, 14, servo6_raw); // Servo output 6 value, in microseconds - put_uint16_t_by_index(msg, 16, servo7_raw); // Servo output 7 value, in microseconds - put_uint16_t_by_index(msg, 18, servo8_raw); // Servo output 8 value, in microseconds - put_uint8_t_by_index(msg, 20, port); // Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. - - mavlink_finalize_message_chan_send(msg, chan, 21, 222); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_uint32_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 4, servo1_raw); + _mav_put_uint16_t(buf, 6, servo2_raw); + _mav_put_uint16_t(buf, 8, servo3_raw); + _mav_put_uint16_t(buf, 10, servo4_raw); + _mav_put_uint16_t(buf, 12, servo5_raw); + _mav_put_uint16_t(buf, 14, servo6_raw); + _mav_put_uint16_t(buf, 16, servo7_raw); + _mav_put_uint16_t(buf, 18, servo8_raw); + _mav_put_uint8_t(buf, 20, port); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, 21, 222); +#else + mavlink_servo_output_raw_t packet; + packet.time_usec = time_usec; + packet.servo1_raw = servo1_raw; + packet.servo2_raw = servo2_raw; + packet.servo3_raw = servo3_raw; + packet.servo4_raw = servo4_raw; + packet.servo5_raw = servo5_raw; + packet.servo6_raw = servo6_raw; + packet.servo7_raw = servo7_raw; + packet.servo8_raw = servo8_raw; + packet.port = port; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, 21, 222); +#endif } #endif @@ -174,7 +224,7 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin */ static inline uint32_t mavlink_msg_servo_output_raw_get_time_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -184,7 +234,7 @@ static inline uint32_t mavlink_msg_servo_output_raw_get_time_usec(const mavlink_ */ static inline uint8_t mavlink_msg_servo_output_raw_get_port(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -194,7 +244,7 @@ static inline uint8_t mavlink_msg_servo_output_raw_get_port(const mavlink_messag */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -204,7 +254,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo1_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -214,7 +264,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo2_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -224,7 +274,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo3_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -234,7 +284,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo4_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -244,7 +294,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo5_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -254,7 +304,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo6_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -264,7 +314,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo7_raw(const mavlink */ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** @@ -287,6 +337,6 @@ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg); #else - memcpy(servo_output_raw, MAVLINK_PAYLOAD(msg), 21); + memcpy(servo_output_raw, _MAV_PAYLOAD(msg), 21); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h new file mode 100644 index 000000000..879cb8d33 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h @@ -0,0 +1,166 @@ +// MESSAGE SET_FLIGHT_MODE PACKING + +#define MAVLINK_MSG_ID_SET_FLIGHT_MODE 12 + +typedef struct __mavlink_set_flight_mode_t +{ + uint8_t target; ///< The system setting the mode + uint8_t flight_mode; ///< The new navigation mode +} mavlink_set_flight_mode_t; + +#define MAVLINK_MSG_ID_SET_FLIGHT_MODE_LEN 2 +#define MAVLINK_MSG_ID_12_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE { \ + "SET_FLIGHT_MODE", \ + 2, \ + { { "target", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_flight_mode_t, target) }, \ + { "flight_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_flight_mode_t, flight_mode) }, \ + } \ +} + + +/** + * @brief Pack a set_flight_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target The system setting the mode + * @param flight_mode The new navigation mode + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_flight_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, uint8_t flight_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, flight_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_set_flight_mode_t packet; + packet.target = target; + packet.flight_mode = flight_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; + return mavlink_finalize_message(msg, system_id, component_id, 2, 194); +} + +/** + * @brief Pack a set_flight_mode message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the mode + * @param flight_mode The new navigation mode + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_flight_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t flight_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, flight_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_set_flight_mode_t packet; + packet.target = target; + packet.flight_mode = flight_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 194); +} + +/** + * @brief Encode a set_flight_mode struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_flight_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_flight_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_flight_mode_t* set_flight_mode) +{ + return mavlink_msg_set_flight_mode_pack(system_id, component_id, msg, set_flight_mode->target, set_flight_mode->flight_mode); +} + +/** + * @brief Send a set_flight_mode message + * @param chan MAVLink channel to send the message + * + * @param target The system setting the mode + * @param flight_mode The new navigation mode + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_flight_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t flight_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, flight_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_FLIGHT_MODE, buf, 2, 194); +#else + mavlink_set_flight_mode_t packet; + packet.target = target; + packet.flight_mode = flight_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_FLIGHT_MODE, (const char *)&packet, 2, 194); +#endif +} + +#endif + +// MESSAGE SET_FLIGHT_MODE UNPACKING + + +/** + * @brief Get field target from set_flight_mode message + * + * @return The system setting the mode + */ +static inline uint8_t mavlink_msg_set_flight_mode_get_target(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field flight_mode from set_flight_mode message + * + * @return The new navigation mode + */ +static inline uint8_t mavlink_msg_set_flight_mode_get_flight_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a set_flight_mode message into a struct + * + * @param msg The message to decode + * @param set_flight_mode C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_flight_mode_decode(const mavlink_message_t* msg, mavlink_set_flight_mode_t* set_flight_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_flight_mode->target = mavlink_msg_set_flight_mode_get_target(msg); + set_flight_mode->flight_mode = mavlink_msg_set_flight_mode_get_flight_mode(msg); +#else + memcpy(set_flight_mode, _MAV_PAYLOAD(msg), 2); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_global_position_setpoint_int.h b/thirdParty/mavlink/include/common/mavlink_msg_set_global_position_setpoint_int.h index fdb546b51..157975e48 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_global_position_setpoint_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_global_position_setpoint_int.h @@ -41,13 +41,25 @@ typedef struct __mavlink_set_global_position_setpoint_int_t static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) { - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_set_global_position_setpoint_int_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.yaw = yaw; - put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 - put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 - put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) - put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; return mavlink_finalize_message(msg, system_id, component_id, 14, 53); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_pack_chan(ui mavlink_message_t* msg, int32_t latitude,int32_t longitude,int32_t altitude,int16_t yaw) { - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 14); +#else + mavlink_set_global_position_setpoint_int_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.yaw = yaw; - put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 - put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 - put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) - put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 + memcpy(_MAV_PAYLOAD(msg), &packet, 14); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 53); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_set_global_position_setpoint_int_encode(uint8 static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, int16_t yaw) { - MAVLINK_ALIGNED_MESSAGE(msg, 14); - msg->msgid = MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT; - - put_int32_t_by_index(msg, 0, latitude); // WGS84 Latitude position in degrees * 1E7 - put_int32_t_by_index(msg, 4, longitude); // WGS84 Longitude position in degrees * 1E7 - put_int32_t_by_index(msg, 8, altitude); // WGS84 Altitude in meters * 1000 (positive for up) - put_int16_t_by_index(msg, 12, yaw); // Desired yaw angle in degrees * 100 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[14]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_int16_t(buf, 12, yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, buf, 14, 53); +#else + mavlink_set_global_position_setpoint_int_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.yaw = yaw; - mavlink_finalize_message_chan_send(msg, chan, 14, 53); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GLOBAL_POSITION_SETPOINT_INT, (const char *)&packet, 14, 53); +#endif } #endif @@ -126,7 +158,7 @@ static inline void mavlink_msg_set_global_position_setpoint_int_send(mavlink_cha */ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -136,7 +168,7 @@ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_latitude( */ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -146,7 +178,7 @@ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_longitude */ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_altitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -156,7 +188,7 @@ static inline int32_t mavlink_msg_set_global_position_setpoint_int_get_altitude( */ static inline int16_t mavlink_msg_set_global_position_setpoint_int_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_set_global_position_setpoint_int_decode(const mav set_global_position_setpoint_int->altitude = mavlink_msg_set_global_position_setpoint_int_get_altitude(msg); set_global_position_setpoint_int->yaw = mavlink_msg_set_global_position_setpoint_int_get_yaw(msg); #else - memcpy(set_global_position_setpoint_int, MAVLINK_PAYLOAD(msg), 14); + memcpy(set_global_position_setpoint_int, _MAV_PAYLOAD(msg), 14); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_gps_global_origin.h b/thirdParty/mavlink/include/common/mavlink_msg_set_gps_global_origin.h index d0ebec8f0..d72b7576c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_gps_global_origin.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_gps_global_origin.h @@ -41,13 +41,25 @@ typedef struct __mavlink_set_gps_global_origin_t static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) { - msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[13]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + + memcpy(_MAV_PAYLOAD(msg), buf, 13); +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; - put_int32_t_by_index(msg, 0, latitude); // global position * 1E7 - put_int32_t_by_index(msg, 4, longitude); // global position * 1E7 - put_int32_t_by_index(msg, 8, altitude); // global position * 1000 - put_uint8_t_by_index(msg, 12, target_system); // System ID + memcpy(_MAV_PAYLOAD(msg), &packet, 13); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; return mavlink_finalize_message(msg, system_id, component_id, 13, 41); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste mavlink_message_t* msg, uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude) { - msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[13]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + + memcpy(_MAV_PAYLOAD(msg), buf, 13); +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; - put_int32_t_by_index(msg, 0, latitude); // global position * 1E7 - put_int32_t_by_index(msg, 4, longitude); // global position * 1E7 - put_int32_t_by_index(msg, 8, altitude); // global position * 1000 - put_uint8_t_by_index(msg, 12, target_system); // System ID + memcpy(_MAV_PAYLOAD(msg), &packet, 13); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 41); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) { - MAVLINK_ALIGNED_MESSAGE(msg, 13); - msg->msgid = MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN; - - put_int32_t_by_index(msg, 0, latitude); // global position * 1E7 - put_int32_t_by_index(msg, 4, longitude); // global position * 1E7 - put_int32_t_by_index(msg, 8, altitude); // global position * 1000 - put_uint8_t_by_index(msg, 12, target_system); // System ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[13]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint8_t(buf, 12, target_system); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, 13, 41); +#else + mavlink_set_gps_global_origin_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude = altitude; + packet.target_system = target_system; - mavlink_finalize_message_chan_send(msg, chan, 13, 41); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, 13, 41); +#endif } #endif @@ -126,7 +158,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_set_gps_global_origin_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 12); + return _MAV_RETURN_uint8_t(msg, 12); } /** @@ -136,7 +168,7 @@ static inline uint8_t mavlink_msg_set_gps_global_origin_get_target_system(const */ static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -146,7 +178,7 @@ static inline int32_t mavlink_msg_set_gps_global_origin_get_latitude(const mavli */ static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 4); + return _MAV_RETURN_int32_t(msg, 4); } /** @@ -156,7 +188,7 @@ static inline int32_t mavlink_msg_set_gps_global_origin_get_longitude(const mavl */ static inline int32_t mavlink_msg_set_gps_global_origin_get_altitude(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 8); + return _MAV_RETURN_int32_t(msg, 8); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_messag set_gps_global_origin->altitude = mavlink_msg_set_gps_global_origin_get_altitude(msg); set_gps_global_origin->target_system = mavlink_msg_set_gps_global_origin_get_target_system(msg); #else - memcpy(set_gps_global_origin, MAVLINK_PAYLOAD(msg), 13); + memcpy(set_gps_global_origin, _MAV_PAYLOAD(msg), 13); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_local_position_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_set_local_position_setpoint.h index 1e63afa22..aaedc0093 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_local_position_setpoint.h @@ -47,15 +47,29 @@ typedef struct __mavlink_set_local_position_setpoint_t static inline uint16_t mavlink_msg_set_local_position_setpoint_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) { - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // Desired yaw angle - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_set_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; return mavlink_finalize_message(msg, system_id, component_id, 18, 131); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_pack_chan(uint8_t mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,float x,float y,float z,float yaw) { - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // Desired yaw angle - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_set_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 131); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_set_local_position_setpoint_encode(uint8_t sy static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // Desired yaw angle - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 18, 131); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, buf, 18, 131); +#else + mavlink_set_local_position_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT, (const char *)&packet, 18, 131); +#endif } #endif @@ -142,7 +180,7 @@ static inline void mavlink_msg_set_local_position_setpoint_send(mavlink_channel_ */ static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -152,7 +190,7 @@ static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_system( */ static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -162,7 +200,7 @@ static inline uint8_t mavlink_msg_set_local_position_setpoint_get_target_compone */ static inline float mavlink_msg_set_local_position_setpoint_get_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -172,7 +210,7 @@ static inline float mavlink_msg_set_local_position_setpoint_get_x(const mavlink_ */ static inline float mavlink_msg_set_local_position_setpoint_get_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -182,7 +220,7 @@ static inline float mavlink_msg_set_local_position_setpoint_get_y(const mavlink_ */ static inline float mavlink_msg_set_local_position_setpoint_get_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -192,7 +230,7 @@ static inline float mavlink_msg_set_local_position_setpoint_get_z(const mavlink_ */ static inline float mavlink_msg_set_local_position_setpoint_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_set_local_position_setpoint_decode(const mavlink_ set_local_position_setpoint->target_system = mavlink_msg_set_local_position_setpoint_get_target_system(msg); set_local_position_setpoint->target_component = mavlink_msg_set_local_position_setpoint_get_target_component(msg); #else - memcpy(set_local_position_setpoint, MAVLINK_PAYLOAD(msg), 18); + memcpy(set_local_position_setpoint, _MAV_PAYLOAD(msg), 18); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index 82c12576b..d109c3692 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -38,12 +38,23 @@ 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_system, uint8_t base_mode, uint16_t custom_mode) { - msg->msgid = MAVLINK_MSG_ID_SET_MODE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, base_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; - put_uint16_t_by_index(msg, 0, custom_mode); // The new autopilot-specific mode. This field can be ignored by an autopilot. - put_uint8_t_by_index(msg, 2, target_system); // The system setting the mode - put_uint8_t_by_index(msg, 3, base_mode); // The new base mode + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_MODE; return mavlink_finalize_message(msg, system_id, component_id, 4, 197); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, uint8_t target_system,uint8_t base_mode,uint16_t custom_mode) { - msg->msgid = MAVLINK_MSG_ID_SET_MODE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, base_mode); - put_uint16_t_by_index(msg, 0, custom_mode); // The new autopilot-specific mode. This field can be ignored by an autopilot. - put_uint8_t_by_index(msg, 2, target_system); // The system setting the mode - put_uint8_t_by_index(msg, 3, base_mode); // The new base mode + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_MODE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 197); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target_system, uint8_t base_mode, uint16_t custom_mode) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_SET_MODE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, custom_mode); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, base_mode); - put_uint16_t_by_index(msg, 0, custom_mode); // The new autopilot-specific mode. This field can be ignored by an autopilot. - put_uint8_t_by_index(msg, 2, target_system); // The system setting the mode - put_uint8_t_by_index(msg, 3, base_mode); // The new base mode + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, buf, 4, 197); +#else + mavlink_set_mode_t packet; + packet.custom_mode = custom_mode; + packet.target_system = target_system; + packet.base_mode = base_mode; - mavlink_finalize_message_chan_send(msg, chan, 4, 197); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_MODE, (const char *)&packet, 4, 197); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_set_mode_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_set_mode_get_target_system(const mavlink_messa */ static inline uint8_t mavlink_msg_set_mode_get_base_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_set_mode_get_base_mode(const mavlink_message_t */ static inline uint16_t mavlink_msg_set_mode_get_custom_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_set_mode_decode(const mavlink_message_t* msg, mav set_mode->target_system = mavlink_msg_set_mode_get_target_system(msg); set_mode->base_mode = mavlink_msg_set_mode_get_base_mode(msg); #else - memcpy(set_mode, MAVLINK_PAYLOAD(msg), 4); + memcpy(set_mode, _MAV_PAYLOAD(msg), 4); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h index 14a95ca47..3c0f72528 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -47,15 +47,29 @@ typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - - put_float_by_index(msg, 0, roll_speed); // Desired roll angular speed in rad/s - put_float_by_index(msg, 4, pitch_speed); // Desired pitch angular speed in rad/s - put_float_by_index(msg, 8, yaw_speed); // Desired yaw angular speed in rad/s - put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, roll_speed); + _mav_put_float(buf, 4, pitch_speed); + _mav_put_float(buf, 8, yaw_speed); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t packet; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; return mavlink_finalize_message(msg, system_id, component_id, 18, 24); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,float roll_speed,float pitch_speed,float yaw_speed,float thrust) { - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - - put_float_by_index(msg, 0, roll_speed); // Desired roll angular speed in rad/s - put_float_by_index(msg, 4, pitch_speed); // Desired pitch angular speed in rad/s - put_float_by_index(msg, 8, yaw_speed); // Desired yaw angular speed in rad/s - put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, roll_speed); + _mav_put_float(buf, 4, pitch_speed); + _mav_put_float(buf, 8, yaw_speed); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t packet; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 24); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; - - put_float_by_index(msg, 0, roll_speed); // Desired roll angular speed in rad/s - put_float_by_index(msg, 4, pitch_speed); // Desired pitch angular speed in rad/s - put_float_by_index(msg, 8, yaw_speed); // Desired yaw angular speed in rad/s - put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 18, 24); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, roll_speed); + _mav_put_float(buf, 4, pitch_speed); + _mav_put_float(buf, 8, yaw_speed); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, buf, 18, 24); +#else + mavlink_set_roll_pitch_yaw_speed_thrust_t packet; + packet.roll_speed = roll_speed; + packet.pitch_speed = pitch_speed; + packet.yaw_speed = yaw_speed; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, (const char *)&packet, 18, 24); +#endif } #endif @@ -142,7 +180,7 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_chan */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -152,7 +190,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_sys */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -162,7 +200,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_com */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -172,7 +210,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(c */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -182,7 +220,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed( */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -192,7 +230,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(co */ static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavl set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg); set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg); #else - memcpy(set_roll_pitch_yaw_speed_thrust, MAVLINK_PAYLOAD(msg), 18); + memcpy(set_roll_pitch_yaw_speed_thrust, _MAV_PAYLOAD(msg), 18); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h index 2933c950d..102e9f2a2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -47,15 +47,29 @@ typedef struct __mavlink_set_roll_pitch_yaw_thrust_t static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) { - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - - put_float_by_index(msg, 0, roll); // Desired roll angle in radians - put_float_by_index(msg, 4, pitch); // Desired pitch angle in radians - put_float_by_index(msg, 8, yaw); // Desired yaw angle in radians - put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_thrust_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; return mavlink_finalize_message(msg, system_id, component_id, 18, 100); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,float roll,float pitch,float yaw,float thrust) { - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - - put_float_by_index(msg, 0, roll); // Desired roll angle in radians - put_float_by_index(msg, 4, pitch); // Desired pitch angle in radians - put_float_by_index(msg, 8, yaw); // Desired yaw angle in radians - put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_set_roll_pitch_yaw_thrust_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 100); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t syst static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) { - MAVLINK_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; - - put_float_by_index(msg, 0, roll); // Desired roll angle in radians - put_float_by_index(msg, 4, pitch); // Desired pitch angle in radians - put_float_by_index(msg, 8, yaw); // Desired yaw angle in radians - put_float_by_index(msg, 12, thrust); // Collective thrust, normalized to 0 .. 1 - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 18, 100); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, buf, 18, 100); +#else + mavlink_set_roll_pitch_yaw_thrust_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, (const char *)&packet, 18, 100); +#endif } #endif @@ -142,7 +180,7 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -152,7 +190,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(co */ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -162,7 +200,7 @@ static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -172,7 +210,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -182,7 +220,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlin */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -192,7 +230,7 @@ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_ */ static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_me set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg); set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg); #else - memcpy(set_roll_pitch_yaw_thrust, MAVLINK_PAYLOAD(msg), 18); + memcpy(set_roll_pitch_yaw_thrust, _MAV_PAYLOAD(msg), 18); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h new file mode 100644 index 000000000..48d563a74 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h @@ -0,0 +1,166 @@ +// MESSAGE SET_SAFETY_MODE PACKING + +#define MAVLINK_MSG_ID_SET_SAFETY_MODE 13 + +typedef struct __mavlink_set_safety_mode_t +{ + uint8_t target; ///< The system setting the mode + uint8_t safety_mode; ///< The new safety mode. The MAV will reject some mode changes during flight. +} mavlink_set_safety_mode_t; + +#define MAVLINK_MSG_ID_SET_SAFETY_MODE_LEN 2 +#define MAVLINK_MSG_ID_13_LEN 2 + + + +#define MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE { \ + "SET_SAFETY_MODE", \ + 2, \ + { { "target", MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_set_safety_mode_t, target) }, \ + { "safety_mode", MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_set_safety_mode_t, safety_mode) }, \ + } \ +} + + +/** + * @brief Pack a set_safety_mode message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target The system setting the mode + * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_safety_mode_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target, uint8_t safety_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, safety_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_set_safety_mode_t packet; + packet.target = target; + packet.safety_mode = safety_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; + return mavlink_finalize_message(msg, system_id, component_id, 2, 8); +} + +/** + * @brief Pack a set_safety_mode message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target The system setting the mode + * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_safety_mode_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target,uint8_t safety_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, safety_mode); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_set_safety_mode_t packet; + packet.target = target; + packet.safety_mode = safety_mode; + + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 8); +} + +/** + * @brief Encode a set_safety_mode struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_safety_mode C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_safety_mode_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_safety_mode_t* set_safety_mode) +{ + return mavlink_msg_set_safety_mode_pack(system_id, component_id, msg, set_safety_mode->target, set_safety_mode->safety_mode); +} + +/** + * @brief Send a set_safety_mode message + * @param chan MAVLink channel to send the message + * + * @param target The system setting the mode + * @param safety_mode The new safety mode. The MAV will reject some mode changes during flight. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_set_safety_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t safety_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target); + _mav_put_uint8_t(buf, 1, safety_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_SAFETY_MODE, buf, 2, 8); +#else + mavlink_set_safety_mode_t packet; + packet.target = target; + packet.safety_mode = safety_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_SAFETY_MODE, (const char *)&packet, 2, 8); +#endif +} + +#endif + +// MESSAGE SET_SAFETY_MODE UNPACKING + + +/** + * @brief Get field target from set_safety_mode message + * + * @return The system setting the mode + */ +static inline uint8_t mavlink_msg_set_safety_mode_get_target(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field safety_mode from set_safety_mode message + * + * @return The new safety mode. The MAV will reject some mode changes during flight. + */ +static inline uint8_t mavlink_msg_set_safety_mode_get_safety_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a set_safety_mode message into a struct + * + * @param msg The message to decode + * @param set_safety_mode C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_safety_mode_decode(const mavlink_message_t* msg, mavlink_set_safety_mode_t* set_safety_mode) +{ +#if MAVLINK_NEED_BYTE_SWAP + set_safety_mode->target = mavlink_msg_set_safety_mode_get_target(msg); + set_safety_mode->safety_mode = mavlink_msg_set_safety_mode_get_safety_mode(msg); +#else + memcpy(set_safety_mode, _MAV_PAYLOAD(msg), 2); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index 2390df0e6..16079ad64 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -56,18 +56,35 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + + memcpy(_MAV_PAYLOAD(msg), buf, 36); +#else + mavlink_state_correction_t packet; + packet.xErr = xErr; + packet.yErr = yErr; + packet.zErr = zErr; + packet.rollErr = rollErr; + packet.pitchErr = pitchErr; + packet.yawErr = yawErr; + packet.vxErr = vxErr; + packet.vyErr = vyErr; + packet.vzErr = vzErr; - put_float_by_index(msg, 0, xErr); // x position error - put_float_by_index(msg, 4, yErr); // y position error - put_float_by_index(msg, 8, zErr); // z position error - put_float_by_index(msg, 12, rollErr); // roll error (radians) - put_float_by_index(msg, 16, pitchErr); // pitch error (radians) - put_float_by_index(msg, 20, yawErr); // yaw error (radians) - put_float_by_index(msg, 24, vxErr); // x velocity - put_float_by_index(msg, 28, vyErr); // y velocity - put_float_by_index(msg, 32, vzErr); // z velocity + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; return mavlink_finalize_message(msg, system_id, component_id, 36, 130); } @@ -92,18 +109,35 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, mavlink_message_t* msg, float xErr,float yErr,float zErr,float rollErr,float pitchErr,float yawErr,float vxErr,float vyErr,float vzErr) { - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); + + memcpy(_MAV_PAYLOAD(msg), buf, 36); +#else + mavlink_state_correction_t packet; + packet.xErr = xErr; + packet.yErr = yErr; + packet.zErr = zErr; + packet.rollErr = rollErr; + packet.pitchErr = pitchErr; + packet.yawErr = yawErr; + packet.vxErr = vxErr; + packet.vyErr = vyErr; + packet.vzErr = vzErr; - put_float_by_index(msg, 0, xErr); // x position error - put_float_by_index(msg, 4, yErr); // y position error - put_float_by_index(msg, 8, zErr); // z position error - put_float_by_index(msg, 12, rollErr); // roll error (radians) - put_float_by_index(msg, 16, pitchErr); // pitch error (radians) - put_float_by_index(msg, 20, yawErr); // yaw error (radians) - put_float_by_index(msg, 24, vxErr); // x velocity - put_float_by_index(msg, 28, vyErr); // y velocity - put_float_by_index(msg, 32, vzErr); // z velocity + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 130); } @@ -138,20 +172,33 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { - MAVLINK_ALIGNED_MESSAGE(msg, 36); - msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, xErr); + _mav_put_float(buf, 4, yErr); + _mav_put_float(buf, 8, zErr); + _mav_put_float(buf, 12, rollErr); + _mav_put_float(buf, 16, pitchErr); + _mav_put_float(buf, 20, yawErr); + _mav_put_float(buf, 24, vxErr); + _mav_put_float(buf, 28, vyErr); + _mav_put_float(buf, 32, vzErr); - put_float_by_index(msg, 0, xErr); // x position error - put_float_by_index(msg, 4, yErr); // y position error - put_float_by_index(msg, 8, zErr); // z position error - put_float_by_index(msg, 12, rollErr); // roll error (radians) - put_float_by_index(msg, 16, pitchErr); // pitch error (radians) - put_float_by_index(msg, 20, yawErr); // yaw error (radians) - put_float_by_index(msg, 24, vxErr); // x velocity - put_float_by_index(msg, 28, vyErr); // y velocity - put_float_by_index(msg, 32, vzErr); // z velocity - - mavlink_finalize_message_chan_send(msg, chan, 36, 130); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, buf, 36, 130); +#else + mavlink_state_correction_t packet; + packet.xErr = xErr; + packet.yErr = yErr; + packet.zErr = zErr; + packet.rollErr = rollErr; + packet.pitchErr = pitchErr; + packet.yawErr = yawErr; + packet.vxErr = vxErr; + packet.vyErr = vyErr; + packet.vzErr = vzErr; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATE_CORRECTION, (const char *)&packet, 36, 130); +#endif } #endif @@ -166,7 +213,7 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo */ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -176,7 +223,7 @@ static inline float mavlink_msg_state_correction_get_xErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -186,7 +233,7 @@ static inline float mavlink_msg_state_correction_get_yErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -196,7 +243,7 @@ static inline float mavlink_msg_state_correction_get_zErr(const mavlink_message_ */ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -206,7 +253,7 @@ static inline float mavlink_msg_state_correction_get_rollErr(const mavlink_messa */ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -216,7 +263,7 @@ static inline float mavlink_msg_state_correction_get_pitchErr(const mavlink_mess */ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -226,7 +273,7 @@ static inline float mavlink_msg_state_correction_get_yawErr(const mavlink_messag */ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -236,7 +283,7 @@ static inline float mavlink_msg_state_correction_get_vxErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -246,7 +293,7 @@ static inline float mavlink_msg_state_correction_get_vyErr(const mavlink_message */ static inline float mavlink_msg_state_correction_get_vzErr(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 32); + return _MAV_RETURN_float(msg, 32); } /** @@ -268,6 +315,6 @@ static inline void mavlink_msg_state_correction_decode(const mavlink_message_t* state_correction->vyErr = mavlink_msg_state_correction_get_vyErr(msg); state_correction->vzErr = mavlink_msg_state_correction_get_vzErr(msg); #else - memcpy(state_correction, MAVLINK_PAYLOAD(msg), 36); + memcpy(state_correction, _MAV_PAYLOAD(msg), 36); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index f190be544..0d6d588fc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -35,11 +35,19 @@ typedef struct __mavlink_statustext_t 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) { - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - - put_uint8_t_by_index(msg, 0, severity); // Severity of status, 0 = info message, 255 = critical fault - put_char_array_by_index(msg, 1, text, 50); // Status text message, without null termination character +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[51]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD(msg), buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + memcpy(packet.text, text, sizeof(char)*50); + memcpy(_MAV_PAYLOAD(msg), &packet, 51); +#endif + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; return mavlink_finalize_message(msg, system_id, component_id, 51, 83); } @@ -57,11 +65,19 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 mavlink_message_t* msg, uint8_t severity,const char *text) { - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - - put_uint8_t_by_index(msg, 0, severity); // Severity of status, 0 = info message, 255 = critical fault - put_char_array_by_index(msg, 1, text, 50); // Status text message, without null termination character +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[51]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); + memcpy(_MAV_PAYLOAD(msg), buf, 51); +#else + mavlink_statustext_t packet; + packet.severity = severity; + memcpy(packet.text, text, sizeof(char)*50); + memcpy(_MAV_PAYLOAD(msg), &packet, 51); +#endif + msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51, 83); } @@ -89,13 +105,17 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) { - MAVLINK_ALIGNED_MESSAGE(msg, 51); - msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - - put_uint8_t_by_index(msg, 0, severity); // Severity of status, 0 = info message, 255 = critical fault - put_char_array_by_index(msg, 1, text, 50); // Status text message, without null termination character - - mavlink_finalize_message_chan_send(msg, chan, 51, 83); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[51]; + _mav_put_uint8_t(buf, 0, severity); + _mav_put_char_array(buf, 1, text, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, 51, 83); +#else + mavlink_statustext_t packet; + packet.severity = severity; + memcpy(packet.text, text, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, 51, 83); +#endif } #endif @@ -110,7 +130,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s */ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +140,7 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_ */ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char *text) { - return MAVLINK_MSG_RETURN_char_array(msg, text, 50, 1); + return _MAV_RETURN_char_array(msg, text, 50, 1); } /** @@ -135,6 +155,6 @@ static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, m statustext->severity = mavlink_msg_statustext_get_severity(msg); mavlink_msg_statustext_get_text(msg, statustext->text); #else - memcpy(statustext, MAVLINK_PAYLOAD(msg), 51); + memcpy(statustext, _MAV_PAYLOAD(msg), 51); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index 1bad53970..793724fbe 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -11,22 +11,23 @@ typedef struct __mavlink_sys_status_t uint16_t voltage_battery; ///< Battery voltage, in millivolts (1 = 1 millivolt) int16_t current_battery; ///< Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current uint16_t watt; ///< Watts consumed from this battery since startup + uint16_t drop_rate_comm; ///< Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) uint16_t errors_comm; ///< Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) uint16_t errors_count1; ///< Autopilot-specific errors uint16_t errors_count2; ///< Autopilot-specific errors uint16_t errors_count3; ///< Autopilot-specific errors uint16_t errors_count4; ///< Autopilot-specific errors - int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + int8_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery } mavlink_sys_status_t; -#define MAVLINK_MSG_ID_SYS_STATUS_LEN 31 -#define MAVLINK_MSG_ID_1_LEN 31 +#define MAVLINK_MSG_ID_SYS_STATUS_LEN 33 +#define MAVLINK_MSG_ID_1_LEN 33 #define MAVLINK_MESSAGE_INFO_SYS_STATUS { \ "SYS_STATUS", \ - 13, \ + 14, \ { { "onboard_control_sensors_present", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_sys_status_t, onboard_control_sensors_present) }, \ { "onboard_control_sensors_enabled", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_sys_status_t, onboard_control_sensors_enabled) }, \ { "onboard_control_sensors_health", MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_sys_status_t, onboard_control_sensors_health) }, \ @@ -34,12 +35,13 @@ typedef struct __mavlink_sys_status_t { "voltage_battery", MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_sys_status_t, voltage_battery) }, \ { "current_battery", MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_sys_status_t, current_battery) }, \ { "watt", MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_sys_status_t, watt) }, \ - { "errors_comm", MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, errors_comm) }, \ - { "errors_count1", MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_count1) }, \ - { "errors_count2", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count2) }, \ - { "errors_count3", MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count3) }, \ - { "errors_count4", MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count4) }, \ - { "battery_remaining", MAVLINK_TYPE_INT8_T, 0, 30, offsetof(mavlink_sys_status_t, battery_remaining) }, \ + { "drop_rate_comm", MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_sys_status_t, drop_rate_comm) }, \ + { "errors_comm", MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_sys_status_t, errors_comm) }, \ + { "errors_count1", MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_sys_status_t, errors_count1) }, \ + { "errors_count2", MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_sys_status_t, errors_count2) }, \ + { "errors_count3", MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_sys_status_t, errors_count3) }, \ + { "errors_count4", MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_sys_status_t, errors_count4) }, \ + { "battery_remaining", MAVLINK_TYPE_INT8_T, 0, 32, offsetof(mavlink_sys_status_t, battery_remaining) }, \ } \ } @@ -57,7 +59,8 @@ typedef struct __mavlink_sys_status_t * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current * @param watt Watts consumed from this battery since startup - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) * @param errors_count1 Autopilot-specific errors * @param errors_count2 Autopilot-specific errors @@ -66,25 +69,48 @@ typedef struct __mavlink_sys_status_t * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, uint16_t watt, int8_t battery_remaining, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) + uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, uint16_t watt, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) { - msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, watt); + _mav_put_uint16_t(buf, 20, drop_rate_comm); + _mav_put_uint16_t(buf, 22, errors_comm); + _mav_put_uint16_t(buf, 24, errors_count1); + _mav_put_uint16_t(buf, 26, errors_count2); + _mav_put_uint16_t(buf, 28, errors_count3); + _mav_put_uint16_t(buf, 30, errors_count4); + _mav_put_int8_t(buf, 32, battery_remaining); + + memcpy(_MAV_PAYLOAD(msg), buf, 33); +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.watt = watt; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + + memcpy(_MAV_PAYLOAD(msg), &packet, 33); +#endif - put_uint32_t_by_index(msg, 0, onboard_control_sensors_present); // Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint32_t_by_index(msg, 4, onboard_control_sensors_enabled); // Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint32_t_by_index(msg, 8, onboard_control_sensors_health); // Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint16_t_by_index(msg, 12, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 10'000) should be always below 10'000 - put_uint16_t_by_index(msg, 14, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) - put_int16_t_by_index(msg, 16, current_battery); // Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - put_uint16_t_by_index(msg, 18, watt); // Watts consumed from this battery since startup - put_uint16_t_by_index(msg, 20, errors_comm); // Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - put_uint16_t_by_index(msg, 22, errors_count1); // Autopilot-specific errors - put_uint16_t_by_index(msg, 24, errors_count2); // Autopilot-specific errors - put_uint16_t_by_index(msg, 26, errors_count3); // Autopilot-specific errors - put_uint16_t_by_index(msg, 28, errors_count4); // Autopilot-specific errors - put_int8_t_by_index(msg, 30, battery_remaining); // Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery - - return mavlink_finalize_message(msg, system_id, component_id, 31, 236); + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 33, 28); } /** @@ -100,7 +126,8 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current * @param watt Watts consumed from this battery since startup - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) * @param errors_count1 Autopilot-specific errors * @param errors_count2 Autopilot-specific errors @@ -110,25 +137,48 @@ 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, - uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,uint16_t watt,int8_t battery_remaining,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4) + uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,uint16_t watt,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4) { - msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, watt); + _mav_put_uint16_t(buf, 20, drop_rate_comm); + _mav_put_uint16_t(buf, 22, errors_comm); + _mav_put_uint16_t(buf, 24, errors_count1); + _mav_put_uint16_t(buf, 26, errors_count2); + _mav_put_uint16_t(buf, 28, errors_count3); + _mav_put_uint16_t(buf, 30, errors_count4); + _mav_put_int8_t(buf, 32, battery_remaining); + + memcpy(_MAV_PAYLOAD(msg), buf, 33); +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.watt = watt; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + + memcpy(_MAV_PAYLOAD(msg), &packet, 33); +#endif - put_uint32_t_by_index(msg, 0, onboard_control_sensors_present); // Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint32_t_by_index(msg, 4, onboard_control_sensors_enabled); // Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint32_t_by_index(msg, 8, onboard_control_sensors_health); // Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint16_t_by_index(msg, 12, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 10'000) should be always below 10'000 - put_uint16_t_by_index(msg, 14, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) - put_int16_t_by_index(msg, 16, current_battery); // Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - put_uint16_t_by_index(msg, 18, watt); // Watts consumed from this battery since startup - put_uint16_t_by_index(msg, 20, errors_comm); // Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - put_uint16_t_by_index(msg, 22, errors_count1); // Autopilot-specific errors - put_uint16_t_by_index(msg, 24, errors_count2); // Autopilot-specific errors - put_uint16_t_by_index(msg, 26, errors_count3); // Autopilot-specific errors - put_uint16_t_by_index(msg, 28, errors_count4); // Autopilot-specific errors - put_int8_t_by_index(msg, 30, battery_remaining); // Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery - - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 31, 236); + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 28); } /** @@ -141,7 +191,7 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status) { - return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->watt, sys_status->battery_remaining, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); + return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->watt, sys_status->battery_remaining, sys_status->drop_rate_comm, sys_status->errors_comm, sys_status->errors_count1, sys_status->errors_count2, sys_status->errors_count3, sys_status->errors_count4); } /** @@ -155,7 +205,8 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current * @param watt Watts consumed from this battery since startup - * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) * @param errors_count1 Autopilot-specific errors * @param errors_count2 Autopilot-specific errors @@ -164,26 +215,45 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, uint16_t watt, int8_t battery_remaining, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) +static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t onboard_control_sensors_present, uint32_t onboard_control_sensors_enabled, uint32_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, int16_t current_battery, uint16_t watt, int8_t battery_remaining, uint16_t drop_rate_comm, uint16_t errors_comm, uint16_t errors_count1, uint16_t errors_count2, uint16_t errors_count3, uint16_t errors_count4) { - MAVLINK_ALIGNED_MESSAGE(msg, 31); - msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - - put_uint32_t_by_index(msg, 0, onboard_control_sensors_present); // Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint32_t_by_index(msg, 4, onboard_control_sensors_enabled); // Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint32_t_by_index(msg, 8, onboard_control_sensors_health); // Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control - put_uint16_t_by_index(msg, 12, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 10'000) should be always below 10'000 - put_uint16_t_by_index(msg, 14, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) - put_int16_t_by_index(msg, 16, current_battery); // Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current - put_uint16_t_by_index(msg, 18, watt); // Watts consumed from this battery since startup - put_uint16_t_by_index(msg, 20, errors_comm); // Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) - put_uint16_t_by_index(msg, 22, errors_count1); // Autopilot-specific errors - put_uint16_t_by_index(msg, 24, errors_count2); // Autopilot-specific errors - put_uint16_t_by_index(msg, 26, errors_count3); // Autopilot-specific errors - put_uint16_t_by_index(msg, 28, errors_count4); // Autopilot-specific errors - put_int8_t_by_index(msg, 30, battery_remaining); // Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery - - mavlink_finalize_message_chan_send(msg, chan, 31, 236); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[33]; + _mav_put_uint32_t(buf, 0, onboard_control_sensors_present); + _mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled); + _mav_put_uint32_t(buf, 8, onboard_control_sensors_health); + _mav_put_uint16_t(buf, 12, load); + _mav_put_uint16_t(buf, 14, voltage_battery); + _mav_put_int16_t(buf, 16, current_battery); + _mav_put_uint16_t(buf, 18, watt); + _mav_put_uint16_t(buf, 20, drop_rate_comm); + _mav_put_uint16_t(buf, 22, errors_comm); + _mav_put_uint16_t(buf, 24, errors_count1); + _mav_put_uint16_t(buf, 26, errors_count2); + _mav_put_uint16_t(buf, 28, errors_count3); + _mav_put_uint16_t(buf, 30, errors_count4); + _mav_put_int8_t(buf, 32, battery_remaining); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, buf, 33, 28); +#else + mavlink_sys_status_t packet; + packet.onboard_control_sensors_present = onboard_control_sensors_present; + packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled; + packet.onboard_control_sensors_health = onboard_control_sensors_health; + packet.load = load; + packet.voltage_battery = voltage_battery; + packet.current_battery = current_battery; + packet.watt = watt; + packet.drop_rate_comm = drop_rate_comm; + packet.errors_comm = errors_comm; + packet.errors_count1 = errors_count1; + packet.errors_count2 = errors_count2; + packet.errors_count3 = errors_count3; + packet.errors_count4 = errors_count4; + packet.battery_remaining = battery_remaining; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_STATUS, (const char *)&packet, 33, 28); +#endif } #endif @@ -198,7 +268,7 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint32_t */ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -208,7 +278,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_presen */ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 4); + return _MAV_RETURN_uint32_t(msg, 4); } /** @@ -218,7 +288,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_enable */ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 8); + return _MAV_RETURN_uint32_t(msg, 8); } /** @@ -228,7 +298,7 @@ static inline uint32_t mavlink_msg_sys_status_get_onboard_control_sensors_health */ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -238,7 +308,7 @@ static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* */ static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 14); + return _MAV_RETURN_uint16_t(msg, 14); } /** @@ -248,7 +318,7 @@ static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_ */ static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -258,17 +328,27 @@ static inline int16_t mavlink_msg_sys_status_get_current_battery(const mavlink_m */ static inline uint16_t mavlink_msg_sys_status_get_watt(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** * @brief Get field battery_remaining from sys_status message * - * @return Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + * @return Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery */ static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int8_t(msg, 30); + return _MAV_RETURN_int8_t(msg, 32); +} + +/** + * @brief Get field drop_rate_comm from sys_status message + * + * @return Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) + */ +static inline uint16_t mavlink_msg_sys_status_get_drop_rate_comm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); } /** @@ -278,7 +358,7 @@ static inline int8_t mavlink_msg_sys_status_get_battery_remaining(const mavlink_ */ static inline uint16_t mavlink_msg_sys_status_get_errors_comm(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 20); + return _MAV_RETURN_uint16_t(msg, 22); } /** @@ -288,7 +368,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_comm(const mavlink_mess */ static inline uint16_t mavlink_msg_sys_status_get_errors_count1(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 22); + return _MAV_RETURN_uint16_t(msg, 24); } /** @@ -298,7 +378,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_count1(const mavlink_me */ static inline uint16_t mavlink_msg_sys_status_get_errors_count2(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 26); } /** @@ -308,7 +388,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_count2(const mavlink_me */ static inline uint16_t mavlink_msg_sys_status_get_errors_count3(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 26); + return _MAV_RETURN_uint16_t(msg, 28); } /** @@ -318,7 +398,7 @@ static inline uint16_t mavlink_msg_sys_status_get_errors_count3(const mavlink_me */ static inline uint16_t mavlink_msg_sys_status_get_errors_count4(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 28); + return _MAV_RETURN_uint16_t(msg, 30); } /** @@ -337,6 +417,7 @@ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, m sys_status->voltage_battery = mavlink_msg_sys_status_get_voltage_battery(msg); sys_status->current_battery = mavlink_msg_sys_status_get_current_battery(msg); sys_status->watt = mavlink_msg_sys_status_get_watt(msg); + sys_status->drop_rate_comm = mavlink_msg_sys_status_get_drop_rate_comm(msg); sys_status->errors_comm = mavlink_msg_sys_status_get_errors_comm(msg); sys_status->errors_count1 = mavlink_msg_sys_status_get_errors_count1(msg); sys_status->errors_count2 = mavlink_msg_sys_status_get_errors_count2(msg); @@ -344,6 +425,6 @@ static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, m sys_status->errors_count4 = mavlink_msg_sys_status_get_errors_count4(msg); sys_status->battery_remaining = mavlink_msg_sys_status_get_battery_remaining(msg); #else - memcpy(sys_status, MAVLINK_PAYLOAD(msg), 31); + memcpy(sys_status, _MAV_PAYLOAD(msg), 33); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index 1b441e3f8..7135a8569 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -35,11 +35,21 @@ 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_unix_usec, uint32_t time_boot_ms) { - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; - put_uint64_t_by_index(msg, 0, time_unix_usec); // Timestamp of the master clock in microseconds since UNIX epoch. - put_uint32_t_by_index(msg, 8, time_boot_ms); // Timestamp of the component clock since boot time in milliseconds. + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; return mavlink_finalize_message(msg, system_id, component_id, 12, 137); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, uint64_t time_unix_usec,uint32_t time_boot_ms) { - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); - put_uint64_t_by_index(msg, 0, time_unix_usec); // Timestamp of the master clock in microseconds since UNIX epoch. - put_uint32_t_by_index(msg, 8, time_boot_ms); // Timestamp of the component clock since boot time in milliseconds. + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 137); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_unix_usec, uint32_t time_boot_ms) { - MAVLINK_ALIGNED_MESSAGE(msg, 12); - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_uint64_t(buf, 0, time_unix_usec); + _mav_put_uint32_t(buf, 8, time_boot_ms); - put_uint64_t_by_index(msg, 0, time_unix_usec); // Timestamp of the master clock in microseconds since UNIX epoch. - put_uint32_t_by_index(msg, 8, time_boot_ms); // Timestamp of the component clock since boot time in milliseconds. + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, buf, 12, 137); +#else + mavlink_system_time_t packet; + packet.time_unix_usec = time_unix_usec; + packet.time_boot_ms = time_boot_ms; - mavlink_finalize_message_chan_send(msg, chan, 12, 137); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME, (const char *)&packet, 12, 137); +#endif } #endif @@ -110,7 +136,7 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t */ static inline uint64_t mavlink_msg_system_time_get_time_unix_usec(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -120,7 +146,7 @@ static inline uint64_t mavlink_msg_system_time_get_time_unix_usec(const mavlink_ */ static inline uint32_t mavlink_msg_system_time_get_time_boot_ms(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 8); + return _MAV_RETURN_uint32_t(msg, 8); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_system_time_decode(const mavlink_message_t* msg, system_time->time_unix_usec = mavlink_msg_system_time_get_time_unix_usec(msg); system_time->time_boot_ms = mavlink_msg_system_time_get_time_boot_ms(msg); #else - memcpy(system_time, MAVLINK_PAYLOAD(msg), 12); + memcpy(system_time, _MAV_PAYLOAD(msg), 12); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h new file mode 100644 index 000000000..646e947e4 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -0,0 +1,166 @@ +// MESSAGE SYSTEM_TIME_UTC PACKING + +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 3 + +typedef struct __mavlink_system_time_utc_t +{ + uint32_t utc_date; ///< GPS UTC date ddmmyy + uint32_t utc_time; ///< GPS UTC time hhmmss +} mavlink_system_time_utc_t; + +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 +#define MAVLINK_MSG_ID_3_LEN 8 + + + +#define MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC { \ + "SYSTEM_TIME_UTC", \ + 2, \ + { { "utc_date", MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_system_time_utc_t, utc_date) }, \ + { "utc_time", MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_system_time_utc_t, utc_time) }, \ + } \ +} + + +/** + * @brief Pack a system_time_utc message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t utc_date, uint32_t utc_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, utc_date); + _mav_put_uint32_t(buf, 4, utc_time); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + return mavlink_finalize_message(msg, system_id, component_id, 8, 191); +} + +/** + * @brief Pack a system_time_utc message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t utc_date,uint32_t utc_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, utc_date); + _mav_put_uint32_t(buf, 4, utc_time); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 191); +} + +/** + * @brief Encode a system_time_utc struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param system_time_utc C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_system_time_utc_t* system_time_utc) +{ + return mavlink_msg_system_time_utc_pack(system_id, component_id, msg, system_time_utc->utc_date, system_time_utc->utc_time); +} + +/** + * @brief Send a system_time_utc message + * @param chan MAVLink channel to send the message + * + * @param utc_date GPS UTC date ddmmyy + * @param utc_time GPS UTC time hhmmss + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, utc_date); + _mav_put_uint32_t(buf, 4, utc_time); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, buf, 8, 191); +#else + mavlink_system_time_utc_t packet; + packet.utc_date = utc_date; + packet.utc_time = utc_time; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, (const char *)&packet, 8, 191); +#endif +} + +#endif + +// MESSAGE SYSTEM_TIME_UTC UNPACKING + + +/** + * @brief Get field utc_date from system_time_utc message + * + * @return GPS UTC date ddmmyy + */ +static inline uint32_t mavlink_msg_system_time_utc_get_utc_date(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field utc_time from system_time_utc message + * + * @return GPS UTC time hhmmss + */ +static inline uint32_t mavlink_msg_system_time_utc_get_utc_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Decode a system_time_utc message into a struct + * + * @param msg The message to decode + * @param system_time_utc C-struct to decode the message contents into + */ +static inline void mavlink_msg_system_time_utc_decode(const mavlink_message_t* msg, mavlink_system_time_utc_t* system_time_utc) +{ +#if MAVLINK_NEED_BYTE_SWAP + system_time_utc->utc_date = mavlink_msg_system_time_utc_get_utc_date(msg); + system_time_utc->utc_time = mavlink_msg_system_time_utc_get_utc_time(msg); +#else + memcpy(system_time_utc, _MAV_PAYLOAD(msg), 8); +#endif +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index ff0a81742..b15c89b2f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - - put_float_by_index(msg, 0, airspeed); // Current airspeed in m/s - put_float_by_index(msg, 4, groundspeed); // Current ground speed in m/s - put_float_by_index(msg, 8, alt); // Current altitude (MSL), in meters - put_float_by_index(msg, 12, climb); // Current climb rate in meters/second - put_int16_t_by_index(msg, 16, heading); // Current heading in degrees, in compass units (0..360, 0=north) - put_uint16_t_by_index(msg, 18, throttle); // Current throttle setting in integer percent, 0 to 100 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; return mavlink_finalize_message(msg, system_id, component_id, 20, 20); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb) { - msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - - put_float_by_index(msg, 0, airspeed); // Current airspeed in m/s - put_float_by_index(msg, 4, groundspeed); // Current ground speed in m/s - put_float_by_index(msg, 8, alt); // Current altitude (MSL), in meters - put_float_by_index(msg, 12, climb); // Current climb rate in meters/second - put_int16_t_by_index(msg, 16, heading); // Current heading in degrees, in compass units (0..360, 0=north) - put_uint16_t_by_index(msg, 18, throttle); // Current throttle setting in integer percent, 0 to 100 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_VFR_HUD; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 20); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { - MAVLINK_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - - put_float_by_index(msg, 0, airspeed); // Current airspeed in m/s - put_float_by_index(msg, 4, groundspeed); // Current ground speed in m/s - put_float_by_index(msg, 8, alt); // Current altitude (MSL), in meters - put_float_by_index(msg, 12, climb); // Current climb rate in meters/second - put_int16_t_by_index(msg, 16, heading); // Current heading in degrees, in compass units (0..360, 0=north) - put_uint16_t_by_index(msg, 18, throttle); // Current throttle setting in integer percent, 0 to 100 - - mavlink_finalize_message_chan_send(msg, chan, 20, 20); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, airspeed); + _mav_put_float(buf, 4, groundspeed); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, climb); + _mav_put_int16_t(buf, 16, heading); + _mav_put_uint16_t(buf, 18, throttle); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, buf, 20, 20); +#else + mavlink_vfr_hud_t packet; + packet.airspeed = airspeed; + packet.groundspeed = groundspeed; + packet.alt = alt; + packet.climb = climb; + packet.heading = heading; + packet.throttle = throttle; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VFR_HUD, (const char *)&packet, 20, 20); +#endif } #endif @@ -142,7 +180,7 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe */ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -152,7 +190,7 @@ static inline float mavlink_msg_vfr_hud_get_airspeed(const mavlink_message_t* ms */ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -162,7 +200,7 @@ static inline float mavlink_msg_vfr_hud_get_groundspeed(const mavlink_message_t* */ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -172,7 +210,7 @@ static inline int16_t mavlink_msg_vfr_hud_get_heading(const mavlink_message_t* m */ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** @@ -182,7 +220,7 @@ static inline uint16_t mavlink_msg_vfr_hud_get_throttle(const mavlink_message_t* */ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -192,7 +230,7 @@ static inline float mavlink_msg_vfr_hud_get_alt(const mavlink_message_t* msg) */ static inline float mavlink_msg_vfr_hud_get_climb(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_vfr_hud_decode(const mavlink_message_t* msg, mavl vfr_hud->heading = mavlink_msg_vfr_hud_get_heading(msg); vfr_hud->throttle = mavlink_msg_vfr_hud_get_throttle(msg); #else - memcpy(vfr_hud, MAVLINK_PAYLOAD(msg), 20); + memcpy(vfr_hud, _MAV_PAYLOAD(msg), 20); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index 272fe4e55..9ab479bba 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -71,23 +71,45 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - - put_float_by_index(msg, 0, param1); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - put_float_by_index(msg, 4, param2); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - put_float_by_index(msg, 8, param3); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - put_float_by_index(msg, 12, param4); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - put_float_by_index(msg, 16, x); // PARAM5 / local: x position, global: latitude - put_float_by_index(msg, 20, y); // PARAM6 / y position: global: longitude - put_float_by_index(msg, 24, z); // PARAM7 / z position: global: altitude - put_uint16_t_by_index(msg, 28, seq); // Sequence - put_uint8_t_by_index(msg, 30, target_system); // System ID - put_uint8_t_by_index(msg, 31, target_component); // Component ID - put_uint8_t_by_index(msg, 32, frame); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - put_uint8_t_by_index(msg, 33, command); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - put_uint8_t_by_index(msg, 34, current); // false:0, true:1 - put_uint8_t_by_index(msg, 35, autocontinue); // autocontinue to next wp +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, command); + _mav_put_uint8_t(buf, 34, current); + _mav_put_uint8_t(buf, 35, autocontinue); + + memcpy(_MAV_PAYLOAD(msg), buf, 36); +#else + mavlink_waypoint_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.command = command; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; return mavlink_finalize_message(msg, system_id, component_id, 36, 205); } @@ -117,23 +139,45 @@ static inline uint16_t mavlink_msg_waypoint_pack_chan(uint8_t system_id, uint8_t 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - - put_float_by_index(msg, 0, param1); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - put_float_by_index(msg, 4, param2); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - put_float_by_index(msg, 8, param3); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - put_float_by_index(msg, 12, param4); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - put_float_by_index(msg, 16, x); // PARAM5 / local: x position, global: latitude - put_float_by_index(msg, 20, y); // PARAM6 / y position: global: longitude - put_float_by_index(msg, 24, z); // PARAM7 / z position: global: altitude - put_uint16_t_by_index(msg, 28, seq); // Sequence - put_uint8_t_by_index(msg, 30, target_system); // System ID - put_uint8_t_by_index(msg, 31, target_component); // Component ID - put_uint8_t_by_index(msg, 32, frame); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - put_uint8_t_by_index(msg, 33, command); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - put_uint8_t_by_index(msg, 34, current); // false:0, true:1 - put_uint8_t_by_index(msg, 35, autocontinue); // autocontinue to next wp +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, command); + _mav_put_uint8_t(buf, 34, current); + _mav_put_uint8_t(buf, 35, autocontinue); + + memcpy(_MAV_PAYLOAD(msg), buf, 36); +#else + mavlink_waypoint_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.command = command; + packet.current = current; + packet.autocontinue = autocontinue; + + memcpy(_MAV_PAYLOAD(msg), &packet, 36); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 205); } @@ -173,25 +217,43 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { - MAVLINK_ALIGNED_MESSAGE(msg, 36); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - - put_float_by_index(msg, 0, param1); // PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - put_float_by_index(msg, 4, param2); // PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - put_float_by_index(msg, 8, param3); // PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - put_float_by_index(msg, 12, param4); // PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - put_float_by_index(msg, 16, x); // PARAM5 / local: x position, global: latitude - put_float_by_index(msg, 20, y); // PARAM6 / y position: global: longitude - put_float_by_index(msg, 24, z); // PARAM7 / z position: global: altitude - put_uint16_t_by_index(msg, 28, seq); // Sequence - put_uint8_t_by_index(msg, 30, target_system); // System ID - put_uint8_t_by_index(msg, 31, target_component); // Component ID - put_uint8_t_by_index(msg, 32, frame); // The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - put_uint8_t_by_index(msg, 33, command); // The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - put_uint8_t_by_index(msg, 34, current); // false:0, true:1 - put_uint8_t_by_index(msg, 35, autocontinue); // autocontinue to next wp - - mavlink_finalize_message_chan_send(msg, chan, 36, 205); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[36]; + _mav_put_float(buf, 0, param1); + _mav_put_float(buf, 4, param2); + _mav_put_float(buf, 8, param3); + _mav_put_float(buf, 12, param4); + _mav_put_float(buf, 16, x); + _mav_put_float(buf, 20, y); + _mav_put_float(buf, 24, z); + _mav_put_uint16_t(buf, 28, seq); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 32, frame); + _mav_put_uint8_t(buf, 33, command); + _mav_put_uint8_t(buf, 34, current); + _mav_put_uint8_t(buf, 35, autocontinue); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, buf, 36, 205); +#else + mavlink_waypoint_t packet; + packet.param1 = param1; + packet.param2 = param2; + packet.param3 = param3; + packet.param4 = param4; + packet.x = x; + packet.y = y; + packet.z = z; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + packet.frame = frame; + packet.command = command; + packet.current = current; + packet.autocontinue = autocontinue; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT, (const char *)&packet, 36, 205); +#endif } #endif @@ -206,7 +268,7 @@ static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t tar */ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 30); + return _MAV_RETURN_uint8_t(msg, 30); } /** @@ -216,7 +278,7 @@ static inline uint8_t mavlink_msg_waypoint_get_target_system(const mavlink_messa */ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 31); + return _MAV_RETURN_uint8_t(msg, 31); } /** @@ -226,7 +288,7 @@ static inline uint8_t mavlink_msg_waypoint_get_target_component(const mavlink_me */ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 28); + return _MAV_RETURN_uint16_t(msg, 28); } /** @@ -236,7 +298,7 @@ static inline uint16_t mavlink_msg_waypoint_get_seq(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 32); + return _MAV_RETURN_uint8_t(msg, 32); } /** @@ -246,7 +308,7 @@ static inline uint8_t mavlink_msg_waypoint_get_frame(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 33); + return _MAV_RETURN_uint8_t(msg, 33); } /** @@ -256,7 +318,7 @@ static inline uint8_t mavlink_msg_waypoint_get_command(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 34); + return _MAV_RETURN_uint8_t(msg, 34); } /** @@ -266,7 +328,7 @@ static inline uint8_t mavlink_msg_waypoint_get_current(const mavlink_message_t* */ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 35); + return _MAV_RETURN_uint8_t(msg, 35); } /** @@ -276,7 +338,7 @@ static inline uint8_t mavlink_msg_waypoint_get_autocontinue(const mavlink_messag */ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -286,7 +348,7 @@ static inline float mavlink_msg_waypoint_get_param1(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -296,7 +358,7 @@ static inline float mavlink_msg_waypoint_get_param2(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -306,7 +368,7 @@ static inline float mavlink_msg_waypoint_get_param3(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -316,7 +378,7 @@ static inline float mavlink_msg_waypoint_get_param4(const mavlink_message_t* msg */ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -326,7 +388,7 @@ static inline float mavlink_msg_waypoint_get_x(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -336,7 +398,7 @@ static inline float mavlink_msg_waypoint_get_y(const mavlink_message_t* msg) */ static inline float mavlink_msg_waypoint_get_z(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -363,6 +425,6 @@ static inline void mavlink_msg_waypoint_decode(const mavlink_message_t* msg, mav waypoint->current = mavlink_msg_waypoint_get_current(msg); waypoint->autocontinue = mavlink_msg_waypoint_get_autocontinue(msg); #else - memcpy(waypoint, MAVLINK_PAYLOAD(msg), 36); + memcpy(waypoint, _MAV_PAYLOAD(msg), 36); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 254167e79..cdbecd55d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_waypoint_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID - put_uint8_t_by_index(msg, 2, type); // 0: OK, 1: Error + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; return mavlink_finalize_message(msg, system_id, component_id, 3, 214); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint8_t type) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID - put_uint8_t_by_index(msg, 2, type); // 0: OK, 1: Error + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_waypoint_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 214); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { - MAVLINK_ALIGNED_MESSAGE(msg, 3); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, type); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID - put_uint8_t_by_index(msg, 2, type); // 0: OK, 1: Error + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_ACK, buf, 3, 214); +#else + mavlink_waypoint_ack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.type = type; - mavlink_finalize_message_chan_send(msg, chan, 3, 214); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_ACK, (const char *)&packet, 3, 214); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_system(const mavlink_m */ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_ack_get_target_component(const mavlin */ static inline uint8_t mavlink_msg_waypoint_ack_get_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_waypoint_ack_decode(const mavlink_message_t* msg, waypoint_ack->target_component = mavlink_msg_waypoint_ack_get_target_component(msg); waypoint_ack->type = mavlink_msg_waypoint_ack_get_type(msg); #else - memcpy(waypoint_ack, MAVLINK_PAYLOAD(msg), 3); + memcpy(waypoint_ack, _MAV_PAYLOAD(msg), 3); #endif } 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 7b2449f59..60aacf49b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -35,11 +35,21 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; return mavlink_finalize_message(msg, system_id, component_id, 2, 229); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack_chan(uint8_t system_i mavlink_message_t* msg, uint8_t target_system,uint8_t target_component) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 229); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - MAVLINK_ALIGNED_MESSAGE(msg, 2); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, buf, 2, 229); +#else + mavlink_waypoint_clear_all_t packet; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 2, 229); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, (const char *)&packet, 2, 229); +#endif } #endif @@ -110,7 +136,7 @@ static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, u */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +146,7 @@ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_system(const mav */ static inline uint8_t mavlink_msg_waypoint_clear_all_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_waypoint_clear_all_decode(const mavlink_message_t waypoint_clear_all->target_system = mavlink_msg_waypoint_clear_all_get_target_system(msg); waypoint_clear_all->target_component = mavlink_msg_waypoint_clear_all_get_target_component(msg); #else - memcpy(waypoint_clear_all, MAVLINK_PAYLOAD(msg), 2); + memcpy(waypoint_clear_all, _MAV_PAYLOAD(msg), 2); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 25f68d671..242570671 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_waypoint_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint16_t_by_index(msg, 0, count); // Number of Waypoints in the Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; return mavlink_finalize_message(msg, system_id, component_id, 4, 8); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_waypoint_count_pack_chan(uint8_t system_id, u mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint16_t count) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); - put_uint16_t_by_index(msg, 0, count); // Number of Waypoints in the Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_waypoint_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 8); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, count); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); - put_uint16_t_by_index(msg, 0, count); // Number of Waypoints in the Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, buf, 4, 8); +#else + mavlink_waypoint_count_t packet; + packet.count = count; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 4, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_COUNT, (const char *)&packet, 4, 8); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8 */ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_system(const mavlink */ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_count_get_target_component(const mavl */ static inline uint16_t mavlink_msg_waypoint_count_get_count(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_waypoint_count_decode(const mavlink_message_t* ms waypoint_count->target_system = mavlink_msg_waypoint_count_get_target_system(msg); waypoint_count->target_component = mavlink_msg_waypoint_count_get_target_component(msg); #else - memcpy(waypoint_count, MAVLINK_PAYLOAD(msg), 4); + memcpy(waypoint_count, _MAV_PAYLOAD(msg), 4); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index db6b91893..16c091894 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -32,10 +32,19 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; - put_uint16_t_by_index(msg, 0, seq); // Sequence + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; return mavlink_finalize_message(msg, system_id, component_id, 2, 101); } @@ -52,10 +61,19 @@ static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint16_t seq) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); - put_uint16_t_by_index(msg, 0, seq); // Sequence + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 101); } @@ -82,12 +100,17 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { - MAVLINK_ALIGNED_MESSAGE(msg, 2); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); - put_uint16_t_by_index(msg, 0, seq); // Sequence + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, buf, 2, 101); +#else + mavlink_waypoint_current_t packet; + packet.seq = seq; - mavlink_finalize_message_chan_send(msg, chan, 2, 101); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT, (const char *)&packet, 2, 101); +#endif } #endif @@ -102,7 +125,7 @@ static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_current_get_seq(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -116,6 +139,6 @@ static inline void mavlink_msg_waypoint_current_decode(const mavlink_message_t* #if MAVLINK_NEED_BYTE_SWAP waypoint_current->seq = mavlink_msg_waypoint_current_get_seq(msg); #else - memcpy(waypoint_current, MAVLINK_PAYLOAD(msg), 2); + memcpy(waypoint_current, _MAV_PAYLOAD(msg), 2); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 30c4d74da..3402ce3f0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -32,10 +32,19 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; - put_uint16_t_by_index(msg, 0, seq); // Sequence + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; return mavlink_finalize_message(msg, system_id, component_id, 2, 21); } @@ -52,10 +61,19 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint16_t seq) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); - put_uint16_t_by_index(msg, 0, seq); // Sequence + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 21); } @@ -82,12 +100,17 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { - MAVLINK_ALIGNED_MESSAGE(msg, 2); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint16_t(buf, 0, seq); - put_uint16_t_by_index(msg, 0, seq); // Sequence + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, buf, 2, 21); +#else + mavlink_waypoint_reached_t packet; + packet.seq = seq; - mavlink_finalize_message_chan_send(msg, chan, 2, 21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REACHED, (const char *)&packet, 2, 21); +#endif } #endif @@ -102,7 +125,7 @@ static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uin */ static inline uint16_t mavlink_msg_waypoint_reached_get_seq(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -116,6 +139,6 @@ static inline void mavlink_msg_waypoint_reached_decode(const mavlink_message_t* #if MAVLINK_NEED_BYTE_SWAP waypoint_reached->seq = mavlink_msg_waypoint_reached_get_seq(msg); #else - memcpy(waypoint_reached, MAVLINK_PAYLOAD(msg), 2); + memcpy(waypoint_reached, _MAV_PAYLOAD(msg), 2); #endif } diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index 1a6afa94e..880c5e678 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_waypoint_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint16_t_by_index(msg, 0, seq); // Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; return mavlink_finalize_message(msg, system_id, component_id, 4, 51); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_waypoint_request_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint16_t seq) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); - put_uint16_t_by_index(msg, 0, seq); // Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_waypoint_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 51); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); - put_uint16_t_by_index(msg, 0, seq); // Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, buf, 4, 51); +#else + mavlink_waypoint_request_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 4, 51); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST, (const char *)&packet, 4, 51); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uin */ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_system(const mavli */ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_request_get_target_component(const ma */ static inline uint16_t mavlink_msg_waypoint_request_get_seq(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_waypoint_request_decode(const mavlink_message_t* waypoint_request->target_system = mavlink_msg_waypoint_request_get_target_system(msg); waypoint_request->target_component = mavlink_msg_waypoint_request_get_target_component(msg); #else - memcpy(waypoint_request, MAVLINK_PAYLOAD(msg), 4); + memcpy(waypoint_request, _MAV_PAYLOAD(msg), 4); #endif } 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 70e612eda..eda72f10b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -35,11 +35,21 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; return mavlink_finalize_message(msg, system_id, component_id, 2, 213); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack_chan(uint8_t syste mavlink_message_t* msg, uint8_t target_system,uint8_t target_component) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 2); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 2); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 213); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { - MAVLINK_ALIGNED_MESSAGE(msg, 2); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[2]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, buf, 2, 213); +#else + mavlink_waypoint_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 2, 213); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, (const char *)&packet, 2, 213); +#endif } #endif @@ -110,7 +136,7 @@ static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -120,7 +146,7 @@ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_system(const */ static inline uint8_t mavlink_msg_waypoint_request_list_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_waypoint_request_list_decode(const mavlink_messag waypoint_request_list->target_system = mavlink_msg_waypoint_request_list_get_target_system(msg); waypoint_request_list->target_component = mavlink_msg_waypoint_request_list_get_target_component(msg); #else - memcpy(waypoint_request_list, MAVLINK_PAYLOAD(msg), 2); + memcpy(waypoint_request_list, _MAV_PAYLOAD(msg), 2); #endif } 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 48be628fd..425f0bc71 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_waypoint_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - put_uint16_t_by_index(msg, 0, seq); // Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; return mavlink_finalize_message(msg, system_id, component_id, 4, 80); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack_chan(uint8_t system mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint16_t seq) { - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); - put_uint16_t_by_index(msg, 0, seq); // Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_waypoint_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 80); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, seq); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); - put_uint16_t_by_index(msg, 0, seq); // Sequence - put_uint8_t_by_index(msg, 2, target_system); // System ID - put_uint8_t_by_index(msg, 3, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, buf, 4, 80); +#else + mavlink_waypoint_set_current_t packet; + packet.seq = seq; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 4, 80); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, (const char *)&packet, 4, 80); +#endif } #endif @@ -118,7 +147,7 @@ static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_system(const m */ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -138,7 +167,7 @@ static inline uint8_t mavlink_msg_waypoint_set_current_get_target_component(cons */ static inline uint16_t mavlink_msg_waypoint_set_current_get_seq(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_waypoint_set_current_decode(const mavlink_message waypoint_set_current->target_system = mavlink_msg_waypoint_set_current_get_target_system(msg); waypoint_set_current->target_component = mavlink_msg_waypoint_set_current_get_target_component(msg); #else - memcpy(waypoint_set_current, MAVLINK_PAYLOAD(msg), 4); + memcpy(waypoint_set_current, _MAV_PAYLOAD(msg), 4); #endif } diff --git a/thirdParty/mavlink/include/common/testsuite.h b/thirdParty/mavlink/include/common/testsuite.h index d827cc631..842976c40 100644 --- a/thirdParty/mavlink/include/common/testsuite.h +++ b/thirdParty/mavlink/include/common/testsuite.h @@ -12,24 +12,24 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t); +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_common(system_id, component_id); + mavlink_test_common(system_id, component_id, last_msg); } #endif -static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_heartbeat_t packet2, packet1 = { + mavlink_heartbeat_t packet_in = { 17235, 139, 206, @@ -37,27 +37,52 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) 84, 3, }; - if (sizeof(packet2) != 7) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_heartbeat_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.custom_mode = packet_in.custom_mode; + packet1.type = packet_in.type; + packet1.autopilot = packet_in.autopilot; + packet1.base_mode = packet_in.base_mode; + packet1.system_status = packet_in.system_status; + packet1.mavlink_version = packet_in.mavlink_version; + + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); mavlink_msg_heartbeat_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; icurrent_tx_seq; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid; + status->current_tx_seq++; + checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&checksum, packet, length); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &checksum); #endif + ck[0] = (uint8_t)(checksum & 0xFF); + ck[1] = (uint8_t)(checksum >> 8); + + MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); + _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); + _mavlink_send_uart(chan, packet, length); + _mavlink_send_uart(chan, (const char *)ck, 2); + MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); +} #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS /** @@ -246,7 +263,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa break; case MAVLINK_PARSE_STATE_GOT_MSGID: - rxmsg->payload.u8[status->packet_idx++] = c; + _MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c; mavlink_update_checksum(rxmsg, c); if (status->packet_idx == rxmsg->len) { @@ -446,18 +463,17 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch) } */ -MAVLINK_HELPER void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg) +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len) { #ifdef MAVLINK_SEND_UART_BYTES /* this is the more efficient approach, if the platform defines it */ - MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len); #else /* fallback to one byte at a time */ - uint8_t *buffer = (uint8_t *)&msg->magic; uint16_t i; - for (i = 0; i < MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; i++) { - comm_send_ch(chan, buffer[i]); + for (i = 0; i < len; i++) { + comm_send_ch(chan, (uint8_t)buf[i]); } #endif } diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h index 7b9533064..241fcde6c 100644 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -79,26 +79,14 @@ typedef struct __mavlink_system { } mavlink_system_t; typedef struct __mavlink_message { - uint16_t checksum; /// sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - union { ///< Payload data, ALIGNMENT IMPORTANT ON MCU - char c[MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES]; - int8_t i8[MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES]; - uint8_t u8[MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES]; - int16_t i16[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+1)/2]; - uint16_t u16[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+1)/2]; - int32_t i32[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+3)/4]; - uint32_t u32[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+3)/4]; - int64_t i64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; - uint64_t u64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; - float f[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+3)/4]; - double d[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; - } payload; + uint16_t checksum; /// sent at end of packet + uint8_t magic; ///< protocol magic marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; } mavlink_message_t; typedef enum { @@ -130,14 +118,14 @@ typedef struct __mavlink_field_info { typedef struct __mavlink_message_info { const char *name; // name of the message unsigned num_fields; // how many fields in this message - const mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information + mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information } mavlink_message_info_t; -#define MAVLINK_PAYLOAD(msg) msg->payload.u8 +#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0])) // checksum is immediately after the payload bytes -#define mavlink_ck_a(msg) msg->payload.u8[(msg)->len] -#define mavlink_ck_b(msg) msg->payload.u8[1+(uint16_t)(msg)->len] +#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg)) +#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg)) typedef enum { MAVLINK_COMM_0, diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 4d7053dab..09cee6e17 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -52,17 +52,33 @@ 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, uint8_t mode, uint8_t nav_mode, uint8_t status, uint8_t safety_status, uint8_t link_status) { - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, autopilot); + _mav_put_uint8_t(buf, 2, mode); + _mav_put_uint8_t(buf, 3, nav_mode); + _mav_put_uint8_t(buf, 4, status); + _mav_put_uint8_t(buf, 5, safety_status); + _mav_put_uint8_t(buf, 6, link_status); + _mav_put_uint8_t(buf, 7, 2); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.status = status; + packet.safety_status = safety_status; + packet.link_status = link_status; + packet.mavlink_version = 2; - put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM - put_uint8_t_by_index(msg, 2, mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 3, nav_mode); // Navigation mode, see MAV_NAV_MODE ENUM - put_uint8_t_by_index(msg, 4, status); // System status flag, see MAV_STATUS ENUM - put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - put_uint8_t_by_index(msg, 7, 2); // MAVLink version + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; return mavlink_finalize_message(msg, system_id, component_id, 8, 11); } @@ -85,17 +101,33 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ mavlink_message_t* msg, uint8_t type,uint8_t autopilot,uint8_t mode,uint8_t nav_mode,uint8_t status,uint8_t safety_status,uint8_t link_status) { - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, autopilot); + _mav_put_uint8_t(buf, 2, mode); + _mav_put_uint8_t(buf, 3, nav_mode); + _mav_put_uint8_t(buf, 4, status); + _mav_put_uint8_t(buf, 5, safety_status); + _mav_put_uint8_t(buf, 6, link_status); + _mav_put_uint8_t(buf, 7, 2); - put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM - put_uint8_t_by_index(msg, 2, mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 3, nav_mode); // Navigation mode, see MAV_NAV_MODE ENUM - put_uint8_t_by_index(msg, 4, status); // System status flag, see MAV_STATUS ENUM - put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - put_uint8_t_by_index(msg, 7, 2); // MAVLink version + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.status = status; + packet.safety_status = safety_status; + packet.link_status = link_status; + packet.mavlink_version = 2; + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 11); } @@ -128,19 +160,31 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot, uint8_t mode, uint8_t nav_mode, uint8_t status, uint8_t safety_status, uint8_t link_status) { - MAVLINK_ALIGNED_MESSAGE(msg, 8); - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint8_t(buf, 0, type); + _mav_put_uint8_t(buf, 1, autopilot); + _mav_put_uint8_t(buf, 2, mode); + _mav_put_uint8_t(buf, 3, nav_mode); + _mav_put_uint8_t(buf, 4, status); + _mav_put_uint8_t(buf, 5, safety_status); + _mav_put_uint8_t(buf, 6, link_status); + _mav_put_uint8_t(buf, 7, 2); - put_uint8_t_by_index(msg, 0, type); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - put_uint8_t_by_index(msg, 1, autopilot); // Autopilot type / class. defined in MAV_CLASS ENUM - put_uint8_t_by_index(msg, 2, mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 3, nav_mode); // Navigation mode, see MAV_NAV_MODE ENUM - put_uint8_t_by_index(msg, 4, status); // System status flag, see MAV_STATUS ENUM - put_uint8_t_by_index(msg, 5, safety_status); // System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - put_uint8_t_by_index(msg, 6, link_status); // Bitmask showing which links are ok / enabled. 0 for disabled/non functional, 1: enabled Indices: 0: RC, 1: UART1, 2: UART2, 3: UART3, 4: UART4, 5: UART5, 6: I2C, 7: CAN - put_uint8_t_by_index(msg, 7, 2); // MAVLink version + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, buf, 8, 11); +#else + mavlink_heartbeat_t packet; + packet.type = type; + packet.autopilot = autopilot; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.status = status; + packet.safety_status = safety_status; + packet.link_status = link_status; + packet.mavlink_version = 2; - mavlink_finalize_message_chan_send(msg, chan, 8, 11); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HEARTBEAT, (const char *)&packet, 8, 11); +#endif } #endif @@ -155,7 +199,7 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty */ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -165,7 +209,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -175,7 +219,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_ */ static inline uint8_t mavlink_msg_heartbeat_get_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -185,7 +229,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_mode(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_heartbeat_get_nav_mode(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -195,7 +239,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_nav_mode(const mavlink_message_t */ static inline uint8_t mavlink_msg_heartbeat_get_status(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -205,7 +249,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_status(const mavlink_message_t* */ static inline uint8_t mavlink_msg_heartbeat_get_safety_status(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -215,7 +259,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_safety_status(const mavlink_mess */ static inline uint8_t mavlink_msg_heartbeat_get_link_status(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -225,7 +269,7 @@ static inline uint8_t mavlink_msg_heartbeat_get_link_status(const mavlink_messag */ static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 7); + return _MAV_RETURN_uint8_t(msg, 7); } /** @@ -246,6 +290,6 @@ static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, ma heartbeat->link_status = mavlink_msg_heartbeat_get_link_status(msg); heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg); #else - memcpy(heartbeat, MAVLINK_PAYLOAD(msg), 8); + memcpy(heartbeat, _MAV_PAYLOAD(msg), 8); #endif } diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index b817ee804..8db0eae33 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -20,7 +20,11 @@ extern "C" { #endif #ifndef MAVLINK_MESSAGE_INFO +<<<<<<< HEAD #define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}} +======= +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} +>>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/minimal/testsuite.h b/thirdParty/mavlink/include/minimal/testsuite.h index 355177820..157ab811e 100644 --- a/thirdParty/mavlink/include/minimal/testsuite.h +++ b/thirdParty/mavlink/include/minimal/testsuite.h @@ -12,24 +12,24 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_minimal(uint8_t, uint8_t); +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_minimal(system_id, component_id); + mavlink_test_minimal(system_id, component_id, last_msg); } #endif -static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_heartbeat_t packet2, packet1 = { + mavlink_heartbeat_t packet_in = { 5, 72, 139, @@ -39,24 +39,51 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) 151, 2, }; - if (sizeof(packet2) != 8) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_heartbeat_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.type = packet_in.type; + packet1.autopilot = packet_in.autopilot; + packet1.mode = packet_in.mode; + packet1.nav_mode = packet_in.nav_mode; + packet1.status = packet_in.status; + packet1.safety_status = packet_in.safety_status; + packet1.link_status = packet_in.link_status; + packet1.mavlink_version = packet_in.mavlink_version; + + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); mavlink_msg_heartbeat_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.mode , packet1.nav_mode , packet1.status , packet1.safety_status , packet1.link_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.mode , packet1.nav_mode , packet1.status , packet1.safety_status , packet1.link_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #include "mavlink.h" diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index 5bbc54fa4..952b7d318 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -56,18 +56,35 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + memcpy(_MAV_PAYLOAD(msg), buf, 21); +#else + mavlink_attitude_control_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target = target; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; - put_float_by_index(msg, 0, roll); // roll - put_float_by_index(msg, 4, pitch); // pitch - put_float_by_index(msg, 8, yaw); // yaw - put_float_by_index(msg, 12, thrust); // thrust - put_uint8_t_by_index(msg, 16, target); // The system to be controlled - put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 - put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 - put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 - put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; return mavlink_finalize_message(msg, system_id, component_id, 21, 254); } @@ -92,18 +109,35 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); + + memcpy(_MAV_PAYLOAD(msg), buf, 21); +#else + mavlink_attitude_control_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target = target; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; - put_float_by_index(msg, 0, roll); // roll - put_float_by_index(msg, 4, pitch); // pitch - put_float_by_index(msg, 8, yaw); // yaw - put_float_by_index(msg, 12, thrust); // thrust - put_uint8_t_by_index(msg, 16, target); // The system to be controlled - put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 - put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 - put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 - put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 + memcpy(_MAV_PAYLOAD(msg), &packet, 21); +#endif + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 254); } @@ -138,20 +172,33 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui 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_ALIGNED_MESSAGE(msg, 21); - msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[21]; + _mav_put_float(buf, 0, roll); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, thrust); + _mav_put_uint8_t(buf, 16, target); + _mav_put_uint8_t(buf, 17, roll_manual); + _mav_put_uint8_t(buf, 18, pitch_manual); + _mav_put_uint8_t(buf, 19, yaw_manual); + _mav_put_uint8_t(buf, 20, thrust_manual); - put_float_by_index(msg, 0, roll); // roll - put_float_by_index(msg, 4, pitch); // pitch - put_float_by_index(msg, 8, yaw); // yaw - put_float_by_index(msg, 12, thrust); // thrust - put_uint8_t_by_index(msg, 16, target); // The system to be controlled - put_uint8_t_by_index(msg, 17, roll_manual); // roll control enabled auto:0, manual:1 - put_uint8_t_by_index(msg, 18, pitch_manual); // pitch auto:0, manual:1 - put_uint8_t_by_index(msg, 19, yaw_manual); // yaw auto:0, manual:1 - put_uint8_t_by_index(msg, 20, thrust_manual); // thrust auto:0, manual:1 - - mavlink_finalize_message_chan_send(msg, chan, 21, 254); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, buf, 21, 254); +#else + mavlink_attitude_control_t packet; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.thrust = thrust; + packet.target = target; + packet.roll_manual = roll_manual; + packet.pitch_manual = pitch_manual; + packet.yaw_manual = yaw_manual; + packet.thrust_manual = thrust_manual; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL, (const char *)&packet, 21, 254); +#endif } #endif @@ -166,7 +213,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -176,7 +223,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -186,7 +233,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -196,7 +243,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -206,7 +253,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -216,7 +263,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -226,7 +273,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 18); + return _MAV_RETURN_uint8_t(msg, 18); } /** @@ -236,7 +283,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 19); + return _MAV_RETURN_uint8_t(msg, 19); } /** @@ -246,7 +293,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -268,6 +315,6 @@ static inline void mavlink_msg_attitude_control_decode(const mavlink_message_t* attitude_control->yaw_manual = mavlink_msg_attitude_control_get_yaw_manual(msg); attitude_control->thrust_manual = mavlink_msg_attitude_control_get_thrust_manual(msg); #else - memcpy(attitude_control, MAVLINK_PAYLOAD(msg), 21); + memcpy(attitude_control, _MAV_PAYLOAD(msg), 21); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index 4024c88e7..a1df04bbc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -53,17 +53,31 @@ typedef struct __mavlink_brief_feature_t 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) { - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - - put_float_by_index(msg, 0, x); // x position in m - put_float_by_index(msg, 4, y); // y position in m - put_float_by_index(msg, 8, z); // z position in m - put_float_by_index(msg, 12, response); // Harris operator response at this location - put_uint16_t_by_index(msg, 16, size); // Size in pixels - put_uint16_t_by_index(msg, 18, orientation); // Orientation - put_uint8_t_by_index(msg, 20, orientation_assignment); // Orientation assignment 0: false, 1:true - put_uint8_t_array_by_index(msg, 21, descriptor, 32); // Descriptor +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[53]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, response); + _mav_put_uint16_t(buf, 16, size); + _mav_put_uint16_t(buf, 18, orientation); + _mav_put_uint8_t(buf, 20, orientation_assignment); + _mav_put_uint8_t_array(buf, 21, descriptor, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 53); +#else + mavlink_brief_feature_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.response = response; + packet.size = size; + packet.orientation = orientation; + packet.orientation_assignment = orientation_assignment; + memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 53); +#endif + msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; return mavlink_finalize_message(msg, system_id, component_id, 53, 88); } @@ -87,17 +101,31 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui 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) { - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - - put_float_by_index(msg, 0, x); // x position in m - put_float_by_index(msg, 4, y); // y position in m - put_float_by_index(msg, 8, z); // z position in m - put_float_by_index(msg, 12, response); // Harris operator response at this location - put_uint16_t_by_index(msg, 16, size); // Size in pixels - put_uint16_t_by_index(msg, 18, orientation); // Orientation - put_uint8_t_by_index(msg, 20, orientation_assignment); // Orientation assignment 0: false, 1:true - put_uint8_t_array_by_index(msg, 21, descriptor, 32); // Descriptor +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[53]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, response); + _mav_put_uint16_t(buf, 16, size); + _mav_put_uint16_t(buf, 18, orientation); + _mav_put_uint8_t(buf, 20, orientation_assignment); + _mav_put_uint8_t_array(buf, 21, descriptor, 32); + memcpy(_MAV_PAYLOAD(msg), buf, 53); +#else + mavlink_brief_feature_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.response = response; + packet.size = size; + packet.orientation = orientation; + packet.orientation_assignment = orientation_assignment; + memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD(msg), &packet, 53); +#endif + msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 53, 88); } @@ -131,19 +159,29 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 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) { - MAVLINK_ALIGNED_MESSAGE(msg, 53); - msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - - put_float_by_index(msg, 0, x); // x position in m - put_float_by_index(msg, 4, y); // y position in m - put_float_by_index(msg, 8, z); // z position in m - put_float_by_index(msg, 12, response); // Harris operator response at this location - put_uint16_t_by_index(msg, 16, size); // Size in pixels - put_uint16_t_by_index(msg, 18, orientation); // Orientation - put_uint8_t_by_index(msg, 20, orientation_assignment); // Orientation assignment 0: false, 1:true - put_uint8_t_array_by_index(msg, 21, descriptor, 32); // Descriptor - - mavlink_finalize_message_chan_send(msg, chan, 53, 88); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[53]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, response); + _mav_put_uint16_t(buf, 16, size); + _mav_put_uint16_t(buf, 18, orientation); + _mav_put_uint8_t(buf, 20, orientation_assignment); + _mav_put_uint8_t_array(buf, 21, descriptor, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, buf, 53, 88); +#else + mavlink_brief_feature_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.response = response; + packet.size = size; + packet.orientation = orientation; + packet.orientation_assignment = orientation_assignment; + memcpy(packet.descriptor, descriptor, sizeof(uint8_t)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BRIEF_FEATURE, (const char *)&packet, 53, 88); +#endif } #endif @@ -158,7 +196,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -168,7 +206,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -178,7 +216,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -188,7 +226,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 20); + return _MAV_RETURN_uint8_t(msg, 20); } /** @@ -198,7 +236,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -208,7 +246,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 18); + return _MAV_RETURN_uint16_t(msg, 18); } /** @@ -218,7 +256,7 @@ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_m */ static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t *descriptor) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, descriptor, 32, 21); + return _MAV_RETURN_uint8_t_array(msg, descriptor, 32, 21); } /** @@ -228,7 +266,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -249,6 +287,6 @@ static inline void mavlink_msg_brief_feature_decode(const mavlink_message_t* msg brief_feature->orientation_assignment = mavlink_msg_brief_feature_get_orientation_assignment(msg); mavlink_msg_brief_feature_get_descriptor(msg, brief_feature->descriptor); #else - memcpy(brief_feature, MAVLINK_PAYLOAD(msg), 53); + memcpy(brief_feature, _MAV_PAYLOAD(msg), 53); #endif } 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 4bbba77b0..a33aae28f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -44,14 +44,27 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - - put_uint32_t_by_index(msg, 0, size); // total data size in bytes (set on ACK only) - put_uint8_t_by_index(msg, 4, type); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - put_uint8_t_by_index(msg, 5, packets); // number of packets beeing sent (set on ACK only) - put_uint8_t_by_index(msg, 6, payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - put_uint8_t_by_index(msg, 7, jpg_quality); // JPEG quality out of [1,100] +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, packets); + _mav_put_uint8_t(buf, 6, payload); + _mav_put_uint8_t(buf, 7, jpg_quality); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.type = type; + packet.packets = packets; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; return mavlink_finalize_message(msg, system_id, component_id, 8, 148); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t mavlink_message_t* msg, uint8_t type,uint32_t size,uint8_t packets,uint8_t payload,uint8_t jpg_quality) { - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - - put_uint32_t_by_index(msg, 0, size); // total data size in bytes (set on ACK only) - put_uint8_t_by_index(msg, 4, type); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - put_uint8_t_by_index(msg, 5, packets); // number of packets beeing sent (set on ACK only) - put_uint8_t_by_index(msg, 6, payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - put_uint8_t_by_index(msg, 7, jpg_quality); // JPEG quality out of [1,100] +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, packets); + _mav_put_uint8_t(buf, 6, payload); + _mav_put_uint8_t(buf, 7, jpg_quality); + + memcpy(_MAV_PAYLOAD(msg), buf, 8); +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.type = type; + packet.packets = packets; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + + memcpy(_MAV_PAYLOAD(msg), &packet, 8); +#endif + msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 148); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy 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_ALIGNED_MESSAGE(msg, 8); - msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - - put_uint32_t_by_index(msg, 0, size); // total data size in bytes (set on ACK only) - put_uint8_t_by_index(msg, 4, type); // type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - put_uint8_t_by_index(msg, 5, packets); // number of packets beeing sent (set on ACK only) - put_uint8_t_by_index(msg, 6, payload); // payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - put_uint8_t_by_index(msg, 7, jpg_quality); // JPEG quality out of [1,100] - - mavlink_finalize_message_chan_send(msg, chan, 8, 148); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[8]; + _mav_put_uint32_t(buf, 0, size); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 5, packets); + _mav_put_uint8_t(buf, 6, payload); + _mav_put_uint8_t(buf, 7, jpg_quality); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, buf, 8, 148); +#else + mavlink_data_transmission_handshake_t packet; + packet.size = size; + packet.type = type; + packet.packets = packets; + packet.payload = payload; + packet.jpg_quality = jpg_quality; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE, (const char *)&packet, 8, 148); +#endif } #endif @@ -134,7 +169,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -144,7 +179,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 0); + return _MAV_RETURN_uint32_t(msg, 0); } /** @@ -154,7 +189,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -164,7 +199,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -174,7 +209,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 7); + return _MAV_RETURN_uint8_t(msg, 7); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_data_transmission_handshake_decode(const mavlink_ 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); #else - memcpy(data_transmission_handshake, MAVLINK_PAYLOAD(msg), 8); + memcpy(data_transmission_handshake, _MAV_PAYLOAD(msg), 8); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index efddee57d..d32bb08fc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -35,11 +35,19 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - - put_uint16_t_by_index(msg, 0, seqnr); // sequence number (starting with 0 on every transmission) - put_uint8_t_array_by_index(msg, 2, data, 253); // image data bytes +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[255]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); + memcpy(_MAV_PAYLOAD(msg), buf, 255); +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + memcpy(packet.data, data, sizeof(uint8_t)*253); + memcpy(_MAV_PAYLOAD(msg), &packet, 255); +#endif + msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; return mavlink_finalize_message(msg, system_id, component_id, 255, 223); } @@ -57,11 +65,19 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id mavlink_message_t* msg, uint16_t seqnr,const uint8_t *data) { - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - - put_uint16_t_by_index(msg, 0, seqnr); // sequence number (starting with 0 on every transmission) - put_uint8_t_array_by_index(msg, 2, data, 253); // image data bytes +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[255]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); + memcpy(_MAV_PAYLOAD(msg), buf, 255); +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + memcpy(packet.data, data, sizeof(uint8_t)*253); + memcpy(_MAV_PAYLOAD(msg), &packet, 255); +#endif + msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 223); } @@ -89,13 +105,17 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t *data) { - MAVLINK_ALIGNED_MESSAGE(msg, 255); - msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - - put_uint16_t_by_index(msg, 0, seqnr); // sequence number (starting with 0 on every transmission) - put_uint8_t_array_by_index(msg, 2, data, 253); // image data bytes - - mavlink_finalize_message_chan_send(msg, chan, 255, 223); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[255]; + _mav_put_uint16_t(buf, 0, seqnr); + _mav_put_uint8_t_array(buf, 2, data, 253); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, buf, 255, 223); +#else + mavlink_encapsulated_data_t packet; + packet.seqnr = seqnr; + memcpy(packet.data, data, sizeof(uint8_t)*253); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA, (const char *)&packet, 255, 223); +#endif } #endif @@ -110,7 +130,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -120,7 +140,7 @@ static inline uint16_t mavlink_msg_encapsulated_data_get_seqnr(const mavlink_mes */ static inline uint16_t mavlink_msg_encapsulated_data_get_data(const mavlink_message_t* msg, uint8_t *data) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, data, 253, 2); + return _MAV_RETURN_uint8_t_array(msg, data, 253, 2); } /** @@ -135,6 +155,6 @@ static inline void mavlink_msg_encapsulated_data_decode(const mavlink_message_t* encapsulated_data->seqnr = mavlink_msg_encapsulated_data_get_seqnr(msg); mavlink_msg_encapsulated_data_get_data(msg, encapsulated_data->data); #else - memcpy(encapsulated_data, MAVLINK_PAYLOAD(msg), 255); + memcpy(encapsulated_data, _MAV_PAYLOAD(msg), 255); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index a55ee69b1..eb8488be8 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -98,32 +98,63 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - - put_uint64_t_by_index(msg, 0, cam_id); // Camera id - put_uint64_t_by_index(msg, 8, timestamp); // Timestamp - put_uint64_t_by_index(msg, 16, valid_until); // Until which timestamp this buffer will stay valid - put_uint32_t_by_index(msg, 24, img_seq); // The image sequence number - put_uint32_t_by_index(msg, 28, img_buf_index); // Position of the image in the buffer, starts with 0 - put_uint32_t_by_index(msg, 32, key); // Shared memory area key - put_uint32_t_by_index(msg, 36, exposure); // Exposure time, in microseconds - put_float_by_index(msg, 40, gain); // Camera gain - put_float_by_index(msg, 44, roll); // Roll angle in rad - put_float_by_index(msg, 48, pitch); // Pitch angle in rad - put_float_by_index(msg, 52, yaw); // Yaw angle in rad - put_float_by_index(msg, 56, local_z); // Local frame Z coordinate (height over ground) - put_float_by_index(msg, 60, lat); // GPS X coordinate - put_float_by_index(msg, 64, lon); // GPS Y coordinate - put_float_by_index(msg, 68, alt); // Global frame altitude - put_float_by_index(msg, 72, ground_x); // Ground truth X - put_float_by_index(msg, 76, ground_y); // Ground truth Y - put_float_by_index(msg, 80, ground_z); // Ground truth Z - put_uint16_t_by_index(msg, 84, width); // Image width - put_uint16_t_by_index(msg, 86, height); // Image height - put_uint16_t_by_index(msg, 88, depth); // Image depth - put_uint8_t_by_index(msg, 90, cam_no); // Camera # (starts with 0) - put_uint8_t_by_index(msg, 91, channels); // Image channels +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[92]; + _mav_put_uint64_t(buf, 0, cam_id); + _mav_put_uint64_t(buf, 8, timestamp); + _mav_put_uint64_t(buf, 16, valid_until); + _mav_put_uint32_t(buf, 24, img_seq); + _mav_put_uint32_t(buf, 28, img_buf_index); + _mav_put_uint32_t(buf, 32, key); + _mav_put_uint32_t(buf, 36, exposure); + _mav_put_float(buf, 40, gain); + _mav_put_float(buf, 44, roll); + _mav_put_float(buf, 48, pitch); + _mav_put_float(buf, 52, yaw); + _mav_put_float(buf, 56, local_z); + _mav_put_float(buf, 60, lat); + _mav_put_float(buf, 64, lon); + _mav_put_float(buf, 68, alt); + _mav_put_float(buf, 72, ground_x); + _mav_put_float(buf, 76, ground_y); + _mav_put_float(buf, 80, ground_z); + _mav_put_uint16_t(buf, 84, width); + _mav_put_uint16_t(buf, 86, height); + _mav_put_uint16_t(buf, 88, depth); + _mav_put_uint8_t(buf, 90, cam_no); + _mav_put_uint8_t(buf, 91, channels); + + memcpy(_MAV_PAYLOAD(msg), buf, 92); +#else + mavlink_image_available_t packet; + packet.cam_id = cam_id; + packet.timestamp = timestamp; + packet.valid_until = valid_until; + packet.img_seq = img_seq; + packet.img_buf_index = img_buf_index; + packet.key = key; + packet.exposure = exposure; + packet.gain = gain; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.local_z = local_z; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.ground_x = ground_x; + packet.ground_y = ground_y; + packet.ground_z = ground_z; + packet.width = width; + packet.height = height; + packet.depth = depth; + packet.cam_no = cam_no; + packet.channels = channels; + + memcpy(_MAV_PAYLOAD(msg), &packet, 92); +#endif + msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; return mavlink_finalize_message(msg, system_id, component_id, 92, 224); } @@ -162,32 +193,63 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - - put_uint64_t_by_index(msg, 0, cam_id); // Camera id - put_uint64_t_by_index(msg, 8, timestamp); // Timestamp - put_uint64_t_by_index(msg, 16, valid_until); // Until which timestamp this buffer will stay valid - put_uint32_t_by_index(msg, 24, img_seq); // The image sequence number - put_uint32_t_by_index(msg, 28, img_buf_index); // Position of the image in the buffer, starts with 0 - put_uint32_t_by_index(msg, 32, key); // Shared memory area key - put_uint32_t_by_index(msg, 36, exposure); // Exposure time, in microseconds - put_float_by_index(msg, 40, gain); // Camera gain - put_float_by_index(msg, 44, roll); // Roll angle in rad - put_float_by_index(msg, 48, pitch); // Pitch angle in rad - put_float_by_index(msg, 52, yaw); // Yaw angle in rad - put_float_by_index(msg, 56, local_z); // Local frame Z coordinate (height over ground) - put_float_by_index(msg, 60, lat); // GPS X coordinate - put_float_by_index(msg, 64, lon); // GPS Y coordinate - put_float_by_index(msg, 68, alt); // Global frame altitude - put_float_by_index(msg, 72, ground_x); // Ground truth X - put_float_by_index(msg, 76, ground_y); // Ground truth Y - put_float_by_index(msg, 80, ground_z); // Ground truth Z - put_uint16_t_by_index(msg, 84, width); // Image width - put_uint16_t_by_index(msg, 86, height); // Image height - put_uint16_t_by_index(msg, 88, depth); // Image depth - put_uint8_t_by_index(msg, 90, cam_no); // Camera # (starts with 0) - put_uint8_t_by_index(msg, 91, channels); // Image channels +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[92]; + _mav_put_uint64_t(buf, 0, cam_id); + _mav_put_uint64_t(buf, 8, timestamp); + _mav_put_uint64_t(buf, 16, valid_until); + _mav_put_uint32_t(buf, 24, img_seq); + _mav_put_uint32_t(buf, 28, img_buf_index); + _mav_put_uint32_t(buf, 32, key); + _mav_put_uint32_t(buf, 36, exposure); + _mav_put_float(buf, 40, gain); + _mav_put_float(buf, 44, roll); + _mav_put_float(buf, 48, pitch); + _mav_put_float(buf, 52, yaw); + _mav_put_float(buf, 56, local_z); + _mav_put_float(buf, 60, lat); + _mav_put_float(buf, 64, lon); + _mav_put_float(buf, 68, alt); + _mav_put_float(buf, 72, ground_x); + _mav_put_float(buf, 76, ground_y); + _mav_put_float(buf, 80, ground_z); + _mav_put_uint16_t(buf, 84, width); + _mav_put_uint16_t(buf, 86, height); + _mav_put_uint16_t(buf, 88, depth); + _mav_put_uint8_t(buf, 90, cam_no); + _mav_put_uint8_t(buf, 91, channels); + + memcpy(_MAV_PAYLOAD(msg), buf, 92); +#else + mavlink_image_available_t packet; + packet.cam_id = cam_id; + packet.timestamp = timestamp; + packet.valid_until = valid_until; + packet.img_seq = img_seq; + packet.img_buf_index = img_buf_index; + packet.key = key; + packet.exposure = exposure; + packet.gain = gain; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.local_z = local_z; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.ground_x = ground_x; + packet.ground_y = ground_y; + packet.ground_z = ground_z; + packet.width = width; + packet.height = height; + packet.depth = depth; + packet.cam_no = cam_no; + packet.channels = channels; + + memcpy(_MAV_PAYLOAD(msg), &packet, 92); +#endif + msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 92, 224); } @@ -236,34 +298,61 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin 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_ALIGNED_MESSAGE(msg, 92); - msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - - put_uint64_t_by_index(msg, 0, cam_id); // Camera id - put_uint64_t_by_index(msg, 8, timestamp); // Timestamp - put_uint64_t_by_index(msg, 16, valid_until); // Until which timestamp this buffer will stay valid - put_uint32_t_by_index(msg, 24, img_seq); // The image sequence number - put_uint32_t_by_index(msg, 28, img_buf_index); // Position of the image in the buffer, starts with 0 - put_uint32_t_by_index(msg, 32, key); // Shared memory area key - put_uint32_t_by_index(msg, 36, exposure); // Exposure time, in microseconds - put_float_by_index(msg, 40, gain); // Camera gain - put_float_by_index(msg, 44, roll); // Roll angle in rad - put_float_by_index(msg, 48, pitch); // Pitch angle in rad - put_float_by_index(msg, 52, yaw); // Yaw angle in rad - put_float_by_index(msg, 56, local_z); // Local frame Z coordinate (height over ground) - put_float_by_index(msg, 60, lat); // GPS X coordinate - put_float_by_index(msg, 64, lon); // GPS Y coordinate - put_float_by_index(msg, 68, alt); // Global frame altitude - put_float_by_index(msg, 72, ground_x); // Ground truth X - put_float_by_index(msg, 76, ground_y); // Ground truth Y - put_float_by_index(msg, 80, ground_z); // Ground truth Z - put_uint16_t_by_index(msg, 84, width); // Image width - put_uint16_t_by_index(msg, 86, height); // Image height - put_uint16_t_by_index(msg, 88, depth); // Image depth - put_uint8_t_by_index(msg, 90, cam_no); // Camera # (starts with 0) - put_uint8_t_by_index(msg, 91, channels); // Image channels - - mavlink_finalize_message_chan_send(msg, chan, 92, 224); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[92]; + _mav_put_uint64_t(buf, 0, cam_id); + _mav_put_uint64_t(buf, 8, timestamp); + _mav_put_uint64_t(buf, 16, valid_until); + _mav_put_uint32_t(buf, 24, img_seq); + _mav_put_uint32_t(buf, 28, img_buf_index); + _mav_put_uint32_t(buf, 32, key); + _mav_put_uint32_t(buf, 36, exposure); + _mav_put_float(buf, 40, gain); + _mav_put_float(buf, 44, roll); + _mav_put_float(buf, 48, pitch); + _mav_put_float(buf, 52, yaw); + _mav_put_float(buf, 56, local_z); + _mav_put_float(buf, 60, lat); + _mav_put_float(buf, 64, lon); + _mav_put_float(buf, 68, alt); + _mav_put_float(buf, 72, ground_x); + _mav_put_float(buf, 76, ground_y); + _mav_put_float(buf, 80, ground_z); + _mav_put_uint16_t(buf, 84, width); + _mav_put_uint16_t(buf, 86, height); + _mav_put_uint16_t(buf, 88, depth); + _mav_put_uint8_t(buf, 90, cam_no); + _mav_put_uint8_t(buf, 91, channels); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, buf, 92, 224); +#else + mavlink_image_available_t packet; + packet.cam_id = cam_id; + packet.timestamp = timestamp; + packet.valid_until = valid_until; + packet.img_seq = img_seq; + packet.img_buf_index = img_buf_index; + packet.key = key; + packet.exposure = exposure; + packet.gain = gain; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.local_z = local_z; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.ground_x = ground_x; + packet.ground_y = ground_y; + packet.ground_z = ground_z; + packet.width = width; + packet.height = height; + packet.depth = depth; + packet.cam_no = cam_no; + packet.channels = channels; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE, (const char *)&packet, 92, 224); +#endif } #endif @@ -278,7 +367,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -288,7 +377,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 90); + return _MAV_RETURN_uint8_t(msg, 90); } /** @@ -298,7 +387,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 8); + return _MAV_RETURN_uint64_t(msg, 8); } /** @@ -308,7 +397,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 16); + return _MAV_RETURN_uint64_t(msg, 16); } /** @@ -318,7 +407,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 24); + return _MAV_RETURN_uint32_t(msg, 24); } /** @@ -328,7 +417,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 28); + return _MAV_RETURN_uint32_t(msg, 28); } /** @@ -338,7 +427,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 84); + return _MAV_RETURN_uint16_t(msg, 84); } /** @@ -348,7 +437,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 86); + return _MAV_RETURN_uint16_t(msg, 86); } /** @@ -358,7 +447,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 88); + return _MAV_RETURN_uint16_t(msg, 88); } /** @@ -368,7 +457,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 91); + return _MAV_RETURN_uint8_t(msg, 91); } /** @@ -378,7 +467,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 32); + return _MAV_RETURN_uint32_t(msg, 32); } /** @@ -388,7 +477,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 36); + return _MAV_RETURN_uint32_t(msg, 36); } /** @@ -398,7 +487,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 40); + return _MAV_RETURN_float(msg, 40); } /** @@ -408,7 +497,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 44); + return _MAV_RETURN_float(msg, 44); } /** @@ -418,7 +507,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 48); + return _MAV_RETURN_float(msg, 48); } /** @@ -428,7 +517,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 52); + return _MAV_RETURN_float(msg, 52); } /** @@ -438,7 +527,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 56); + return _MAV_RETURN_float(msg, 56); } /** @@ -448,7 +537,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 60); + return _MAV_RETURN_float(msg, 60); } /** @@ -458,7 +547,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 64); + return _MAV_RETURN_float(msg, 64); } /** @@ -468,7 +557,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 68); + return _MAV_RETURN_float(msg, 68); } /** @@ -478,7 +567,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 72); + return _MAV_RETURN_float(msg, 72); } /** @@ -488,7 +577,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 76); + return _MAV_RETURN_float(msg, 76); } /** @@ -498,7 +587,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 80); + return _MAV_RETURN_float(msg, 80); } /** @@ -534,6 +623,6 @@ static inline void mavlink_msg_image_available_decode(const mavlink_message_t* m image_available->cam_no = mavlink_msg_image_available_get_cam_no(msg); image_available->channels = mavlink_msg_image_available_get_channels(msg); #else - memcpy(image_available, MAVLINK_PAYLOAD(msg), 92); + memcpy(image_available, _MAV_PAYLOAD(msg), 92); #endif } 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 01c8e5379..591cf6aee 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -32,10 +32,19 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[1]; + _mav_put_uint8_t(buf, 0, enable); + + memcpy(_MAV_PAYLOAD(msg), buf, 1); +#else + mavlink_image_trigger_control_t packet; + packet.enable = enable; - put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable + memcpy(_MAV_PAYLOAD(msg), &packet, 1); +#endif + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; return mavlink_finalize_message(msg, system_id, component_id, 1, 95); } @@ -52,10 +61,19 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste mavlink_message_t* msg, uint8_t enable) { - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[1]; + _mav_put_uint8_t(buf, 0, enable); - put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable + memcpy(_MAV_PAYLOAD(msg), buf, 1); +#else + mavlink_image_trigger_control_t packet; + packet.enable = enable; + memcpy(_MAV_PAYLOAD(msg), &packet, 1); +#endif + + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1, 95); } @@ -82,12 +100,17 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { - MAVLINK_ALIGNED_MESSAGE(msg, 1); - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[1]; + _mav_put_uint8_t(buf, 0, enable); - put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, buf, 1, 95); +#else + mavlink_image_trigger_control_t packet; + packet.enable = enable; - mavlink_finalize_message_chan_send(msg, chan, 1, 95); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL, (const char *)&packet, 1, 95); +#endif } #endif @@ -102,7 +125,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -116,6 +139,6 @@ static inline void mavlink_msg_image_trigger_control_decode(const mavlink_messag #if MAVLINK_NEED_BYTE_SWAP image_trigger_control->enable = mavlink_msg_image_trigger_control_get_enable(msg); #else - memcpy(image_trigger_control, MAVLINK_PAYLOAD(msg), 1); + memcpy(image_trigger_control, _MAV_PAYLOAD(msg), 1); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 56c128d32..951982bae 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -65,21 +65,41 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - - put_uint64_t_by_index(msg, 0, timestamp); // Timestamp - put_uint32_t_by_index(msg, 8, seq); // IMU seq - put_float_by_index(msg, 12, roll); // Roll angle in rad - put_float_by_index(msg, 16, pitch); // Pitch angle in rad - put_float_by_index(msg, 20, yaw); // Yaw angle in rad - put_float_by_index(msg, 24, local_z); // Local frame Z coordinate (height over ground) - put_float_by_index(msg, 28, lat); // GPS X coordinate - put_float_by_index(msg, 32, lon); // GPS Y coordinate - put_float_by_index(msg, 36, alt); // Global frame altitude - put_float_by_index(msg, 40, ground_x); // Ground truth X - put_float_by_index(msg, 44, ground_y); // Ground truth Y - put_float_by_index(msg, 48, ground_z); // Ground truth Z +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[52]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_float(buf, 24, local_z); + _mav_put_float(buf, 28, lat); + _mav_put_float(buf, 32, lon); + _mav_put_float(buf, 36, alt); + _mav_put_float(buf, 40, ground_x); + _mav_put_float(buf, 44, ground_y); + _mav_put_float(buf, 48, ground_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 52); +#else + mavlink_image_triggered_t packet; + packet.timestamp = timestamp; + packet.seq = seq; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.local_z = local_z; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.ground_x = ground_x; + packet.ground_y = ground_y; + packet.ground_z = ground_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 52); +#endif + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; return mavlink_finalize_message(msg, system_id, component_id, 52, 86); } @@ -107,21 +127,41 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - - put_uint64_t_by_index(msg, 0, timestamp); // Timestamp - put_uint32_t_by_index(msg, 8, seq); // IMU seq - put_float_by_index(msg, 12, roll); // Roll angle in rad - put_float_by_index(msg, 16, pitch); // Pitch angle in rad - put_float_by_index(msg, 20, yaw); // Yaw angle in rad - put_float_by_index(msg, 24, local_z); // Local frame Z coordinate (height over ground) - put_float_by_index(msg, 28, lat); // GPS X coordinate - put_float_by_index(msg, 32, lon); // GPS Y coordinate - put_float_by_index(msg, 36, alt); // Global frame altitude - put_float_by_index(msg, 40, ground_x); // Ground truth X - put_float_by_index(msg, 44, ground_y); // Ground truth Y - put_float_by_index(msg, 48, ground_z); // Ground truth Z +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[52]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_float(buf, 24, local_z); + _mav_put_float(buf, 28, lat); + _mav_put_float(buf, 32, lon); + _mav_put_float(buf, 36, alt); + _mav_put_float(buf, 40, ground_x); + _mav_put_float(buf, 44, ground_y); + _mav_put_float(buf, 48, ground_z); + + memcpy(_MAV_PAYLOAD(msg), buf, 52); +#else + mavlink_image_triggered_t packet; + packet.timestamp = timestamp; + packet.seq = seq; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.local_z = local_z; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.ground_x = ground_x; + packet.ground_y = ground_y; + packet.ground_z = ground_z; + + memcpy(_MAV_PAYLOAD(msg), &packet, 52); +#endif + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 52, 86); } @@ -159,23 +199,39 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin 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_ALIGNED_MESSAGE(msg, 52); - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - - put_uint64_t_by_index(msg, 0, timestamp); // Timestamp - put_uint32_t_by_index(msg, 8, seq); // IMU seq - put_float_by_index(msg, 12, roll); // Roll angle in rad - put_float_by_index(msg, 16, pitch); // Pitch angle in rad - put_float_by_index(msg, 20, yaw); // Yaw angle in rad - put_float_by_index(msg, 24, local_z); // Local frame Z coordinate (height over ground) - put_float_by_index(msg, 28, lat); // GPS X coordinate - put_float_by_index(msg, 32, lon); // GPS Y coordinate - put_float_by_index(msg, 36, alt); // Global frame altitude - put_float_by_index(msg, 40, ground_x); // Ground truth X - put_float_by_index(msg, 44, ground_y); // Ground truth Y - put_float_by_index(msg, 48, ground_z); // Ground truth Z - - mavlink_finalize_message_chan_send(msg, chan, 52, 86); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[52]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint32_t(buf, 8, seq); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_float(buf, 24, local_z); + _mav_put_float(buf, 28, lat); + _mav_put_float(buf, 32, lon); + _mav_put_float(buf, 36, alt); + _mav_put_float(buf, 40, ground_x); + _mav_put_float(buf, 44, ground_y); + _mav_put_float(buf, 48, ground_z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, buf, 52, 86); +#else + mavlink_image_triggered_t packet; + packet.timestamp = timestamp; + packet.seq = seq; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.local_z = local_z; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.ground_x = ground_x; + packet.ground_y = ground_y; + packet.ground_z = ground_z; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED, (const char *)&packet, 52, 86); +#endif } #endif @@ -190,7 +246,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -200,7 +256,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 8); + return _MAV_RETURN_uint32_t(msg, 8); } /** @@ -210,7 +266,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -220,7 +276,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -230,7 +286,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -240,7 +296,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -250,7 +306,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -260,7 +316,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 32); + return _MAV_RETURN_float(msg, 32); } /** @@ -270,7 +326,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 36); + return _MAV_RETURN_float(msg, 36); } /** @@ -280,7 +336,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 40); + return _MAV_RETURN_float(msg, 40); } /** @@ -290,7 +346,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 44); + return _MAV_RETURN_float(msg, 44); } /** @@ -300,7 +356,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 48); + return _MAV_RETURN_float(msg, 48); } /** @@ -325,6 +381,6 @@ static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* m image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg); image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg); #else - memcpy(image_triggered, MAVLINK_PAYLOAD(msg), 52); + memcpy(image_triggered, _MAV_PAYLOAD(msg), 52); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index dac1c42b6..5af96f6bc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_MARKER; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_uint16_t(buf, 24, id); + + memcpy(_MAV_PAYLOAD(msg), buf, 26); +#else + mavlink_marker_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.id = id; - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, roll); // roll orientation - put_float_by_index(msg, 16, pitch); // pitch orientation - put_float_by_index(msg, 20, yaw); // yaw orientation - put_uint16_t_by_index(msg, 24, id); // ID + memcpy(_MAV_PAYLOAD(msg), &packet, 26); +#endif + msg->msgid = MAVLINK_MSG_ID_MARKER; return mavlink_finalize_message(msg, system_id, component_id, 26, 249); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c mavlink_message_t* msg, uint16_t id,float x,float y,float z,float roll,float pitch,float yaw) { - msg->msgid = MAVLINK_MSG_ID_MARKER; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_uint16_t(buf, 24, id); - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, roll); // roll orientation - put_float_by_index(msg, 16, pitch); // pitch orientation - put_float_by_index(msg, 20, yaw); // yaw orientation - put_uint16_t_by_index(msg, 24, id); // ID + memcpy(_MAV_PAYLOAD(msg), buf, 26); +#else + mavlink_marker_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.id = id; + memcpy(_MAV_PAYLOAD(msg), &packet, 26); +#endif + + msg->msgid = MAVLINK_MSG_ID_MARKER; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 249); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp 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_ALIGNED_MESSAGE(msg, 26); - msg->msgid = MAVLINK_MSG_ID_MARKER; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[26]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, roll); + _mav_put_float(buf, 16, pitch); + _mav_put_float(buf, 20, yaw); + _mav_put_uint16_t(buf, 24, id); - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, roll); // roll orientation - put_float_by_index(msg, 16, pitch); // pitch orientation - put_float_by_index(msg, 20, yaw); // yaw orientation - put_uint16_t_by_index(msg, 24, id); // ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, buf, 26, 249); +#else + mavlink_marker_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.id = id; - mavlink_finalize_message_chan_send(msg, chan, 26, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MARKER, (const char *)&packet, 26, 249); +#endif } #endif @@ -150,7 +191,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 24); } /** @@ -160,7 +201,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -170,7 +211,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -180,7 +221,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -190,7 +231,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -200,7 +241,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -210,7 +251,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_marker_decode(const mavlink_message_t* msg, mavli marker->yaw = mavlink_msg_marker_get_yaw(msg); marker->id = mavlink_msg_marker_get_id(msg); #else - memcpy(marker, MAVLINK_PAYLOAD(msg), 26); + memcpy(marker, _MAV_PAYLOAD(msg), 26); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 24b35b4b4..6c62294c0 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -41,13 +41,23 @@ typedef struct __mavlink_pattern_detected_t 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) { - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - - put_float_by_index(msg, 0, confidence); // Confidence of detection - put_uint8_t_by_index(msg, 4, type); // 0: Pattern, 1: Letter - put_char_array_by_index(msg, 5, file, 100); // Pattern file name - put_uint8_t_by_index(msg, 105, detected); // Accepted as true detection, 0 no, 1 yes +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[106]; + _mav_put_float(buf, 0, confidence); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 105, detected); + _mav_put_char_array(buf, 5, file, 100); + memcpy(_MAV_PAYLOAD(msg), buf, 106); +#else + mavlink_pattern_detected_t packet; + packet.confidence = confidence; + packet.type = type; + packet.detected = detected; + memcpy(packet.file, file, sizeof(char)*100); + memcpy(_MAV_PAYLOAD(msg), &packet, 106); +#endif + msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; return mavlink_finalize_message(msg, system_id, component_id, 106, 90); } @@ -67,13 +77,23 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint8_t type,float confidence,const char *file,uint8_t detected) { - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - - put_float_by_index(msg, 0, confidence); // Confidence of detection - put_uint8_t_by_index(msg, 4, type); // 0: Pattern, 1: Letter - put_char_array_by_index(msg, 5, file, 100); // Pattern file name - put_uint8_t_by_index(msg, 105, detected); // Accepted as true detection, 0 no, 1 yes +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[106]; + _mav_put_float(buf, 0, confidence); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 105, detected); + _mav_put_char_array(buf, 5, file, 100); + memcpy(_MAV_PAYLOAD(msg), buf, 106); +#else + mavlink_pattern_detected_t packet; + packet.confidence = confidence; + packet.type = type; + packet.detected = detected; + memcpy(packet.file, file, sizeof(char)*100); + memcpy(_MAV_PAYLOAD(msg), &packet, 106); +#endif + msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 106, 90); } @@ -103,15 +123,21 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char *file, uint8_t detected) { - MAVLINK_ALIGNED_MESSAGE(msg, 106); - msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - - put_float_by_index(msg, 0, confidence); // Confidence of detection - put_uint8_t_by_index(msg, 4, type); // 0: Pattern, 1: Letter - put_char_array_by_index(msg, 5, file, 100); // Pattern file name - put_uint8_t_by_index(msg, 105, detected); // Accepted as true detection, 0 no, 1 yes - - mavlink_finalize_message_chan_send(msg, chan, 106, 90); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[106]; + _mav_put_float(buf, 0, confidence); + _mav_put_uint8_t(buf, 4, type); + _mav_put_uint8_t(buf, 105, detected); + _mav_put_char_array(buf, 5, file, 100); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, buf, 106, 90); +#else + mavlink_pattern_detected_t packet; + packet.confidence = confidence; + packet.type = type; + packet.detected = detected; + memcpy(packet.file, file, sizeof(char)*100); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PATTERN_DETECTED, (const char *)&packet, 106, 90); +#endif } #endif @@ -126,7 +152,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -136,7 +162,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -146,7 +172,7 @@ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_me */ static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char *file) { - return MAVLINK_MSG_RETURN_char_array(msg, file, 100, 5); + return _MAV_RETURN_char_array(msg, file, 100, 5); } /** @@ -156,7 +182,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 105); + return _MAV_RETURN_uint8_t(msg, 105); } /** @@ -173,6 +199,6 @@ static inline void mavlink_msg_pattern_detected_decode(const mavlink_message_t* mavlink_msg_pattern_detected_get_file(msg, pattern_detected->file); pattern_detected->detected = mavlink_msg_pattern_detected_get_detected(msg); #else - memcpy(pattern_detected, MAVLINK_PAYLOAD(msg), 106); + memcpy(pattern_detected, _MAV_PAYLOAD(msg), 106); #endif } 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 2afaeeaa2..8d28c7bbb 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -53,17 +53,31 @@ typedef struct __mavlink_point_of_interest_t 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) { - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - - put_float_by_index(msg, 0, x); // X Position - put_float_by_index(msg, 4, y); // Y Position - put_float_by_index(msg, 8, z); // Z Position - put_uint16_t_by_index(msg, 12, timeout); // 0: no timeout, >1: timeout in seconds - put_uint8_t_by_index(msg, 14, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - put_uint8_t_by_index(msg, 15, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - put_uint8_t_by_index(msg, 16, coordinate_system); // 0: global, 1:local - put_char_array_by_index(msg, 17, name, 26); // POI name +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[43]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_uint16_t(buf, 12, timeout); + _mav_put_uint8_t(buf, 14, type); + _mav_put_uint8_t(buf, 15, color); + _mav_put_uint8_t(buf, 16, coordinate_system); + _mav_put_char_array(buf, 17, name, 26); + memcpy(_MAV_PAYLOAD(msg), buf, 43); +#else + mavlink_point_of_interest_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.timeout = timeout; + packet.type = type; + packet.color = color; + packet.coordinate_system = coordinate_system; + memcpy(packet.name, name, sizeof(char)*26); + memcpy(_MAV_PAYLOAD(msg), &packet, 43); +#endif + msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; return mavlink_finalize_message(msg, system_id, component_id, 43, 95); } @@ -87,17 +101,31 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - - put_float_by_index(msg, 0, x); // X Position - put_float_by_index(msg, 4, y); // Y Position - put_float_by_index(msg, 8, z); // Z Position - put_uint16_t_by_index(msg, 12, timeout); // 0: no timeout, >1: timeout in seconds - put_uint8_t_by_index(msg, 14, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - put_uint8_t_by_index(msg, 15, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - put_uint8_t_by_index(msg, 16, coordinate_system); // 0: global, 1:local - put_char_array_by_index(msg, 17, name, 26); // POI name +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[43]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_uint16_t(buf, 12, timeout); + _mav_put_uint8_t(buf, 14, type); + _mav_put_uint8_t(buf, 15, color); + _mav_put_uint8_t(buf, 16, coordinate_system); + _mav_put_char_array(buf, 17, name, 26); + memcpy(_MAV_PAYLOAD(msg), buf, 43); +#else + mavlink_point_of_interest_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.timeout = timeout; + packet.type = type; + packet.color = color; + packet.coordinate_system = coordinate_system; + memcpy(packet.name, name, sizeof(char)*26); + memcpy(_MAV_PAYLOAD(msg), &packet, 43); +#endif + msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 43, 95); } @@ -131,19 +159,29 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u 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_ALIGNED_MESSAGE(msg, 43); - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - - put_float_by_index(msg, 0, x); // X Position - put_float_by_index(msg, 4, y); // Y Position - put_float_by_index(msg, 8, z); // Z Position - put_uint16_t_by_index(msg, 12, timeout); // 0: no timeout, >1: timeout in seconds - put_uint8_t_by_index(msg, 14, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - put_uint8_t_by_index(msg, 15, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - put_uint8_t_by_index(msg, 16, coordinate_system); // 0: global, 1:local - put_char_array_by_index(msg, 17, name, 26); // POI name - - mavlink_finalize_message_chan_send(msg, chan, 43, 95); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[43]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_uint16_t(buf, 12, timeout); + _mav_put_uint8_t(buf, 14, type); + _mav_put_uint8_t(buf, 15, color); + _mav_put_uint8_t(buf, 16, coordinate_system); + _mav_put_char_array(buf, 17, name, 26); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, buf, 43, 95); +#else + mavlink_point_of_interest_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.timeout = timeout; + packet.type = type; + packet.color = color; + packet.coordinate_system = coordinate_system; + memcpy(packet.name, name, sizeof(char)*26); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST, (const char *)&packet, 43, 95); +#endif } #endif @@ -158,7 +196,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 14); + return _MAV_RETURN_uint8_t(msg, 14); } /** @@ -168,7 +206,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 15); + return _MAV_RETURN_uint8_t(msg, 15); } /** @@ -178,7 +216,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -188,7 +226,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -198,7 +236,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -208,7 +246,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -218,7 +256,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -228,7 +266,7 @@ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* */ static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char *name) { - return MAVLINK_MSG_RETURN_char_array(msg, name, 26, 17); + return _MAV_RETURN_char_array(msg, name, 26, 17); } /** @@ -249,6 +287,6 @@ static inline void mavlink_msg_point_of_interest_decode(const mavlink_message_t* point_of_interest->coordinate_system = mavlink_msg_point_of_interest_get_coordinate_system(msg); mavlink_msg_point_of_interest_get_name(msg, point_of_interest->name); #else - memcpy(point_of_interest, MAVLINK_PAYLOAD(msg), 43); + memcpy(point_of_interest, _MAV_PAYLOAD(msg), 43); #endif } 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 708aed1fd..6f17a8702 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 @@ -62,20 +62,37 @@ typedef struct __mavlink_point_of_interest_connection_t 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) { - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - - put_float_by_index(msg, 0, xp1); // X1 Position - put_float_by_index(msg, 4, yp1); // Y1 Position - put_float_by_index(msg, 8, zp1); // Z1 Position - put_float_by_index(msg, 12, xp2); // X2 Position - put_float_by_index(msg, 16, yp2); // Y2 Position - put_float_by_index(msg, 20, zp2); // Z2 Position - put_uint16_t_by_index(msg, 24, timeout); // 0: no timeout, >1: timeout in seconds - put_uint8_t_by_index(msg, 26, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - put_uint8_t_by_index(msg, 27, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - put_uint8_t_by_index(msg, 28, coordinate_system); // 0: global, 1:local - put_char_array_by_index(msg, 29, name, 26); // POI connection name +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[55]; + _mav_put_float(buf, 0, xp1); + _mav_put_float(buf, 4, yp1); + _mav_put_float(buf, 8, zp1); + _mav_put_float(buf, 12, xp2); + _mav_put_float(buf, 16, yp2); + _mav_put_float(buf, 20, zp2); + _mav_put_uint16_t(buf, 24, timeout); + _mav_put_uint8_t(buf, 26, type); + _mav_put_uint8_t(buf, 27, color); + _mav_put_uint8_t(buf, 28, coordinate_system); + _mav_put_char_array(buf, 29, name, 26); + memcpy(_MAV_PAYLOAD(msg), buf, 55); +#else + mavlink_point_of_interest_connection_t packet; + packet.xp1 = xp1; + packet.yp1 = yp1; + packet.zp1 = zp1; + packet.xp2 = xp2; + packet.yp2 = yp2; + packet.zp2 = zp2; + packet.timeout = timeout; + packet.type = type; + packet.color = color; + packet.coordinate_system = coordinate_system; + memcpy(packet.name, name, sizeof(char)*26); + memcpy(_MAV_PAYLOAD(msg), &packet, 55); +#endif + msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; return mavlink_finalize_message(msg, system_id, component_id, 55, 36); } @@ -102,20 +119,37 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ 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) { - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - - put_float_by_index(msg, 0, xp1); // X1 Position - put_float_by_index(msg, 4, yp1); // Y1 Position - put_float_by_index(msg, 8, zp1); // Z1 Position - put_float_by_index(msg, 12, xp2); // X2 Position - put_float_by_index(msg, 16, yp2); // Y2 Position - put_float_by_index(msg, 20, zp2); // Z2 Position - put_uint16_t_by_index(msg, 24, timeout); // 0: no timeout, >1: timeout in seconds - put_uint8_t_by_index(msg, 26, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - put_uint8_t_by_index(msg, 27, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - put_uint8_t_by_index(msg, 28, coordinate_system); // 0: global, 1:local - put_char_array_by_index(msg, 29, name, 26); // POI connection name +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[55]; + _mav_put_float(buf, 0, xp1); + _mav_put_float(buf, 4, yp1); + _mav_put_float(buf, 8, zp1); + _mav_put_float(buf, 12, xp2); + _mav_put_float(buf, 16, yp2); + _mav_put_float(buf, 20, zp2); + _mav_put_uint16_t(buf, 24, timeout); + _mav_put_uint8_t(buf, 26, type); + _mav_put_uint8_t(buf, 27, color); + _mav_put_uint8_t(buf, 28, coordinate_system); + _mav_put_char_array(buf, 29, name, 26); + memcpy(_MAV_PAYLOAD(msg), buf, 55); +#else + mavlink_point_of_interest_connection_t packet; + packet.xp1 = xp1; + packet.yp1 = yp1; + packet.zp1 = zp1; + packet.xp2 = xp2; + packet.yp2 = yp2; + packet.zp2 = zp2; + packet.timeout = timeout; + packet.type = type; + packet.color = color; + packet.coordinate_system = coordinate_system; + memcpy(packet.name, name, sizeof(char)*26); + memcpy(_MAV_PAYLOAD(msg), &packet, 55); +#endif + msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 55, 36); } @@ -152,22 +186,35 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s 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_ALIGNED_MESSAGE(msg, 55); - msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - - put_float_by_index(msg, 0, xp1); // X1 Position - put_float_by_index(msg, 4, yp1); // Y1 Position - put_float_by_index(msg, 8, zp1); // Z1 Position - put_float_by_index(msg, 12, xp2); // X2 Position - put_float_by_index(msg, 16, yp2); // Y2 Position - put_float_by_index(msg, 20, zp2); // Z2 Position - put_uint16_t_by_index(msg, 24, timeout); // 0: no timeout, >1: timeout in seconds - put_uint8_t_by_index(msg, 26, type); // 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - put_uint8_t_by_index(msg, 27, color); // 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - put_uint8_t_by_index(msg, 28, coordinate_system); // 0: global, 1:local - put_char_array_by_index(msg, 29, name, 26); // POI connection name - - mavlink_finalize_message_chan_send(msg, chan, 55, 36); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[55]; + _mav_put_float(buf, 0, xp1); + _mav_put_float(buf, 4, yp1); + _mav_put_float(buf, 8, zp1); + _mav_put_float(buf, 12, xp2); + _mav_put_float(buf, 16, yp2); + _mav_put_float(buf, 20, zp2); + _mav_put_uint16_t(buf, 24, timeout); + _mav_put_uint8_t(buf, 26, type); + _mav_put_uint8_t(buf, 27, color); + _mav_put_uint8_t(buf, 28, coordinate_system); + _mav_put_char_array(buf, 29, name, 26); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, buf, 55, 36); +#else + mavlink_point_of_interest_connection_t packet; + packet.xp1 = xp1; + packet.yp1 = yp1; + packet.zp1 = zp1; + packet.xp2 = xp2; + packet.yp2 = yp2; + packet.zp2 = zp2; + packet.timeout = timeout; + packet.type = type; + packet.color = color; + packet.coordinate_system = coordinate_system; + memcpy(packet.name, name, sizeof(char)*26); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION, (const char *)&packet, 55, 36); +#endif } #endif @@ -182,7 +229,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 26); + return _MAV_RETURN_uint8_t(msg, 26); } /** @@ -192,7 +239,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 27); + return _MAV_RETURN_uint8_t(msg, 27); } /** @@ -202,7 +249,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 28); + return _MAV_RETURN_uint8_t(msg, 28); } /** @@ -212,7 +259,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 24); + return _MAV_RETURN_uint16_t(msg, 24); } /** @@ -222,7 +269,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -232,7 +279,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -242,7 +289,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -252,7 +299,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -262,7 +309,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -272,7 +319,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -282,7 +329,7 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavli */ static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char *name) { - return MAVLINK_MSG_RETURN_char_array(msg, name, 26, 29); + return _MAV_RETURN_char_array(msg, name, 26, 29); } /** @@ -306,6 +353,6 @@ static inline void mavlink_msg_point_of_interest_connection_decode(const mavlink point_of_interest_connection->coordinate_system = mavlink_msg_point_of_interest_connection_get_coordinate_system(msg); mavlink_msg_point_of_interest_connection_get_name(msg, point_of_interest_connection->name); #else - memcpy(point_of_interest_connection, MAVLINK_PAYLOAD(msg), 55); + memcpy(point_of_interest_connection, _MAV_PAYLOAD(msg), 55); #endif } 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 4aca8c61c..fa35d7b7a 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 @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - - put_float_by_index(msg, 0, x); // x position offset - put_float_by_index(msg, 4, y); // y position offset - put_float_by_index(msg, 8, z); // z position offset - put_float_by_index(msg, 12, yaw); // yaw orientation offset in radians, 0 = NORTH - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_position_control_offset_set_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; return mavlink_finalize_message(msg, system_id, component_id, 18, 244); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack_chan(uint8_t mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,float x,float y,float z,float yaw) { - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - - put_float_by_index(msg, 0, x); // x position offset - put_float_by_index(msg, 4, y); // y position offset - put_float_by_index(msg, 8, z); // z position offset - put_float_by_index(msg, 12, yaw); // yaw orientation offset in radians, 0 = NORTH - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_position_control_offset_set_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 244); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy 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_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - - put_float_by_index(msg, 0, x); // x position offset - put_float_by_index(msg, 4, y); // y position offset - put_float_by_index(msg, 8, z); // z position offset - put_float_by_index(msg, 12, yaw); // yaw orientation offset in radians, 0 = NORTH - put_uint8_t_by_index(msg, 16, target_system); // System ID - put_uint8_t_by_index(msg, 17, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 18, 244); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint8_t(buf, 16, target_system); + _mav_put_uint8_t(buf, 17, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET, buf, 18, 244); +#else + mavlink_position_control_offset_set_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET, (const char *)&packet, 18, 244); +#endif } #endif @@ -142,7 +180,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 16); + return _MAV_RETURN_uint8_t(msg, 16); } /** @@ -152,7 +190,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 17); + return _MAV_RETURN_uint8_t(msg, 17); } /** @@ -162,7 +200,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -172,7 +210,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -182,7 +220,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -192,7 +230,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_position_control_offset_set_decode(const mavlink_ 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); #else - memcpy(position_control_offset_set, MAVLINK_PAYLOAD(msg), 18); + memcpy(position_control_offset_set, _MAV_PAYLOAD(msg), 18); #endif } 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 2e00c6e40..5172a91df 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -44,14 +44,27 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_position_control_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.id = id; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; return mavlink_finalize_message(msg, system_id, component_id, 18, 28); } @@ -72,14 +85,27 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s mavlink_message_t* msg, uint16_t id,float x,float y,float z,float yaw) { - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_position_control_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.id = id; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 28); } @@ -110,16 +136,25 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst 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_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position - - mavlink_finalize_message_chan_send(msg, chan, 18, 28); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, buf, 18, 28); +#else + mavlink_position_control_setpoint_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.id = id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT, (const char *)&packet, 18, 28); +#endif } #endif @@ -134,7 +169,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -144,7 +179,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -154,7 +189,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -164,7 +199,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -174,7 +209,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -192,6 +227,6 @@ static inline void mavlink_msg_position_control_setpoint_decode(const mavlink_me position_control_setpoint->yaw = mavlink_msg_position_control_setpoint_get_yaw(msg); position_control_setpoint->id = mavlink_msg_position_control_setpoint_get_id(msg); #else - memcpy(position_control_setpoint, MAVLINK_PAYLOAD(msg), 18); + memcpy(position_control_setpoint, _MAV_PAYLOAD(msg), 18); #endif } 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 37de388db..44cf74773 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 @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_position_control_setpoint_set_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position - put_uint8_t_by_index(msg, 18, target_system); // System ID - put_uint8_t_by_index(msg, 19, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; return mavlink_finalize_message(msg, system_id, component_id, 20, 11); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack_chan(uint8 mavlink_message_t* msg, uint8_t target_system,uint8_t target_component,uint16_t id,float x,float y,float z,float yaw) { - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position - put_uint8_t_by_index(msg, 18, target_system); // System ID - put_uint8_t_by_index(msg, 19, target_component); // Component ID + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_position_control_setpoint_set_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 11); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t 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_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_float(buf, 0, x); + _mav_put_float(buf, 4, y); + _mav_put_float(buf, 8, z); + _mav_put_float(buf, 12, yaw); + _mav_put_uint16_t(buf, 16, id); + _mav_put_uint8_t(buf, 18, target_system); + _mav_put_uint8_t(buf, 19, target_component); - put_float_by_index(msg, 0, x); // x position - put_float_by_index(msg, 4, y); // y position - put_float_by_index(msg, 8, z); // z position - put_float_by_index(msg, 12, yaw); // yaw orientation in radians, 0 = NORTH - put_uint16_t_by_index(msg, 16, id); // ID of waypoint, 0 for plain position - put_uint8_t_by_index(msg, 18, target_system); // System ID - put_uint8_t_by_index(msg, 19, target_component); // Component ID + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET, buf, 20, 11); +#else + mavlink_position_control_setpoint_set_t packet; + packet.x = x; + packet.y = y; + packet.z = z; + packet.yaw = yaw; + packet.id = id; + packet.target_system = target_system; + packet.target_component = target_component; - mavlink_finalize_message_chan_send(msg, chan, 20, 11); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET, (const char *)&packet, 20, 11); +#endif } #endif @@ -150,7 +191,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 18); + return _MAV_RETURN_uint8_t(msg, 18); } /** @@ -160,7 +201,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 19); + return _MAV_RETURN_uint8_t(msg, 19); } /** @@ -170,7 +211,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 16); + return _MAV_RETURN_uint16_t(msg, 16); } /** @@ -180,7 +221,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -190,7 +231,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -200,7 +241,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -210,7 +251,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_position_control_setpoint_set_decode(const mavlin 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); #else - memcpy(position_control_setpoint_set, MAVLINK_PAYLOAD(msg), 20); + memcpy(position_control_setpoint_set, _MAV_PAYLOAD(msg), 20); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 68722c5b9..3f780093e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_int32_t(buf, 0, baro); + _mav_put_uint16_t(buf, 4, adc1); + _mav_put_uint16_t(buf, 6, adc2); + _mav_put_uint16_t(buf, 8, adc3); + _mav_put_uint16_t(buf, 10, adc4); + _mav_put_uint16_t(buf, 12, vbat); + _mav_put_int16_t(buf, 14, temp); + + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_raw_aux_t packet; + packet.baro = baro; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.vbat = vbat; + packet.temp = temp; - put_int32_t_by_index(msg, 0, baro); // Barometric pressure (hecto Pascal) - put_uint16_t_by_index(msg, 4, adc1); // ADC1 (J405 ADC3, LPC2148 AD0.6) - put_uint16_t_by_index(msg, 6, adc2); // ADC2 (J405 ADC5, LPC2148 AD0.2) - put_uint16_t_by_index(msg, 8, adc3); // ADC3 (J405 ADC6, LPC2148 AD0.1) - put_uint16_t_by_index(msg, 10, adc4); // ADC4 (J405 ADC7, LPC2148 AD1.3) - put_uint16_t_by_index(msg, 12, vbat); // Battery voltage - put_int16_t_by_index(msg, 14, temp); // Temperature (degrees celcius) + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + msg->msgid = MAVLINK_MSG_ID_RAW_AUX; return mavlink_finalize_message(msg, system_id, component_id, 16, 182); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t 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) { - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_int32_t(buf, 0, baro); + _mav_put_uint16_t(buf, 4, adc1); + _mav_put_uint16_t(buf, 6, adc2); + _mav_put_uint16_t(buf, 8, adc3); + _mav_put_uint16_t(buf, 10, adc4); + _mav_put_uint16_t(buf, 12, vbat); + _mav_put_int16_t(buf, 14, temp); - put_int32_t_by_index(msg, 0, baro); // Barometric pressure (hecto Pascal) - put_uint16_t_by_index(msg, 4, adc1); // ADC1 (J405 ADC3, LPC2148 AD0.6) - put_uint16_t_by_index(msg, 6, adc2); // ADC2 (J405 ADC5, LPC2148 AD0.2) - put_uint16_t_by_index(msg, 8, adc3); // ADC3 (J405 ADC6, LPC2148 AD0.1) - put_uint16_t_by_index(msg, 10, adc4); // ADC4 (J405 ADC7, LPC2148 AD1.3) - put_uint16_t_by_index(msg, 12, vbat); // Battery voltage - put_int16_t_by_index(msg, 14, temp); // Temperature (degrees celcius) + memcpy(_MAV_PAYLOAD(msg), buf, 16); +#else + mavlink_raw_aux_t packet; + packet.baro = baro; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.vbat = vbat; + packet.temp = temp; + memcpy(_MAV_PAYLOAD(msg), &packet, 16); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_AUX; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 182); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com 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_ALIGNED_MESSAGE(msg, 16); - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[16]; + _mav_put_int32_t(buf, 0, baro); + _mav_put_uint16_t(buf, 4, adc1); + _mav_put_uint16_t(buf, 6, adc2); + _mav_put_uint16_t(buf, 8, adc3); + _mav_put_uint16_t(buf, 10, adc4); + _mav_put_uint16_t(buf, 12, vbat); + _mav_put_int16_t(buf, 14, temp); - put_int32_t_by_index(msg, 0, baro); // Barometric pressure (hecto Pascal) - put_uint16_t_by_index(msg, 4, adc1); // ADC1 (J405 ADC3, LPC2148 AD0.6) - put_uint16_t_by_index(msg, 6, adc2); // ADC2 (J405 ADC5, LPC2148 AD0.2) - put_uint16_t_by_index(msg, 8, adc3); // ADC3 (J405 ADC6, LPC2148 AD0.1) - put_uint16_t_by_index(msg, 10, adc4); // ADC4 (J405 ADC7, LPC2148 AD1.3) - put_uint16_t_by_index(msg, 12, vbat); // Battery voltage - put_int16_t_by_index(msg, 14, temp); // Temperature (degrees celcius) + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, buf, 16, 182); +#else + mavlink_raw_aux_t packet; + packet.baro = baro; + packet.adc1 = adc1; + packet.adc2 = adc2; + packet.adc3 = adc3; + packet.adc4 = adc4; + packet.vbat = vbat; + packet.temp = temp; - mavlink_finalize_message_chan_send(msg, chan, 16, 182); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_AUX, (const char *)&packet, 16, 182); +#endif } #endif @@ -150,7 +191,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -160,7 +201,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -170,7 +211,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -180,7 +221,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 10); + return _MAV_RETURN_uint16_t(msg, 10); } /** @@ -190,7 +231,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 12); + return _MAV_RETURN_uint16_t(msg, 12); } /** @@ -200,7 +241,7 @@ 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) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -210,7 +251,7 @@ 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) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_raw_aux_decode(const mavlink_message_t* msg, mavl raw_aux->vbat = mavlink_msg_raw_aux_get_vbat(msg); raw_aux->temp = mavlink_msg_raw_aux_get_temp(msg); #else - memcpy(raw_aux, MAVLINK_PAYLOAD(msg), 16); + memcpy(raw_aux, _MAV_PAYLOAD(msg), 16); #endif } 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 f2957a38b..7bbce1e2d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - - put_float_by_index(msg, 0, gain); // Camera gain - put_uint16_t_by_index(msg, 4, interval); // Shutter interval, in microseconds - put_uint16_t_by_index(msg, 6, exposure); // Exposure time, in microseconds - put_uint8_t_by_index(msg, 8, cam_no); // Camera id - put_uint8_t_by_index(msg, 9, cam_mode); // Camera mode: 0 = auto, 1 = manual - put_uint8_t_by_index(msg, 10, trigger_pin); // Trigger pin, 0-3 for PtGrey FireFly +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[11]; + _mav_put_float(buf, 0, gain); + _mav_put_uint16_t(buf, 4, interval); + _mav_put_uint16_t(buf, 6, exposure); + _mav_put_uint8_t(buf, 8, cam_no); + _mav_put_uint8_t(buf, 9, cam_mode); + _mav_put_uint8_t(buf, 10, trigger_pin); + + memcpy(_MAV_PAYLOAD(msg), buf, 11); +#else + mavlink_set_cam_shutter_t packet; + packet.gain = gain; + packet.interval = interval; + packet.exposure = exposure; + packet.cam_no = cam_no; + packet.cam_mode = cam_mode; + packet.trigger_pin = trigger_pin; + + memcpy(_MAV_PAYLOAD(msg), &packet, 11); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; return mavlink_finalize_message(msg, system_id, component_id, 11, 108); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - - put_float_by_index(msg, 0, gain); // Camera gain - put_uint16_t_by_index(msg, 4, interval); // Shutter interval, in microseconds - put_uint16_t_by_index(msg, 6, exposure); // Exposure time, in microseconds - put_uint8_t_by_index(msg, 8, cam_no); // Camera id - put_uint8_t_by_index(msg, 9, cam_mode); // Camera mode: 0 = auto, 1 = manual - put_uint8_t_by_index(msg, 10, trigger_pin); // Trigger pin, 0-3 for PtGrey FireFly +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[11]; + _mav_put_float(buf, 0, gain); + _mav_put_uint16_t(buf, 4, interval); + _mav_put_uint16_t(buf, 6, exposure); + _mav_put_uint8_t(buf, 8, cam_no); + _mav_put_uint8_t(buf, 9, cam_mode); + _mav_put_uint8_t(buf, 10, trigger_pin); + + memcpy(_MAV_PAYLOAD(msg), buf, 11); +#else + mavlink_set_cam_shutter_t packet; + packet.gain = gain; + packet.interval = interval; + packet.exposure = exposure; + packet.cam_no = cam_no; + packet.cam_mode = cam_mode; + packet.trigger_pin = trigger_pin; + + memcpy(_MAV_PAYLOAD(msg), &packet, 11); +#endif + msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11, 108); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin 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_ALIGNED_MESSAGE(msg, 11); - msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - - put_float_by_index(msg, 0, gain); // Camera gain - put_uint16_t_by_index(msg, 4, interval); // Shutter interval, in microseconds - put_uint16_t_by_index(msg, 6, exposure); // Exposure time, in microseconds - put_uint8_t_by_index(msg, 8, cam_no); // Camera id - put_uint8_t_by_index(msg, 9, cam_mode); // Camera mode: 0 = auto, 1 = manual - put_uint8_t_by_index(msg, 10, trigger_pin); // Trigger pin, 0-3 for PtGrey FireFly - - mavlink_finalize_message_chan_send(msg, chan, 11, 108); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[11]; + _mav_put_float(buf, 0, gain); + _mav_put_uint16_t(buf, 4, interval); + _mav_put_uint16_t(buf, 6, exposure); + _mav_put_uint8_t(buf, 8, cam_no); + _mav_put_uint8_t(buf, 9, cam_mode); + _mav_put_uint8_t(buf, 10, trigger_pin); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, buf, 11, 108); +#else + mavlink_set_cam_shutter_t packet; + packet.gain = gain; + packet.interval = interval; + packet.exposure = exposure; + packet.cam_no = cam_no; + packet.cam_mode = cam_mode; + packet.trigger_pin = trigger_pin; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER, (const char *)&packet, 11, 108); +#endif } #endif @@ -142,7 +180,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 8); + return _MAV_RETURN_uint8_t(msg, 8); } /** @@ -152,7 +190,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 9); + return _MAV_RETURN_uint8_t(msg, 9); } /** @@ -162,7 +200,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 10); + return _MAV_RETURN_uint8_t(msg, 10); } /** @@ -172,7 +210,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -182,7 +220,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -192,7 +230,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_set_cam_shutter_decode(const mavlink_message_t* m 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); #else - memcpy(set_cam_shutter, MAVLINK_PAYLOAD(msg), 11); + memcpy(set_cam_shutter, _MAV_PAYLOAD(msg), 11); #endif } 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 b3c6ee26c..9ebc66673 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X position - put_float_by_index(msg, 12, y); // Global Y position - put_float_by_index(msg, 16, z); // Global Z position - put_float_by_index(msg, 20, roll); // Roll angle in rad - put_float_by_index(msg, 24, pitch); // Pitch angle in rad - put_float_by_index(msg, 28, yaw); // Yaw angle in rad + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; return mavlink_finalize_message(msg, system_id, component_id, 32, 56); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys mavlink_message_t* msg, uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) { - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X position - put_float_by_index(msg, 12, y); // Global Y position - put_float_by_index(msg, 16, z); // Global Z position - put_float_by_index(msg, 20, roll); // Roll angle in rad - put_float_by_index(msg, 24, pitch); // Pitch angle in rad - put_float_by_index(msg, 28, yaw); // Yaw angle in rad + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 56); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system 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_ALIGNED_MESSAGE(msg, 32); - msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X position - put_float_by_index(msg, 12, y); // Global Y position - put_float_by_index(msg, 16, z); // Global Z position - put_float_by_index(msg, 20, roll); // Roll angle in rad - put_float_by_index(msg, 24, pitch); // Pitch angle in rad - put_float_by_index(msg, 28, yaw); // Yaw angle in rad + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, 32, 56); +#else + mavlink_vicon_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; - mavlink_finalize_message_chan_send(msg, chan, 32, 56); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, 32, 56); +#endif } #endif @@ -150,7 +191,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -160,7 +201,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -170,7 +211,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -180,7 +221,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -190,7 +231,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -200,7 +241,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -210,7 +251,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_mess vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); #else - memcpy(vicon_position_estimate, MAVLINK_PAYLOAD(msg), 32); + memcpy(vicon_position_estimate, _MAV_PAYLOAD(msg), 32); #endif } 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 270fc74af..cadb9b829 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X position - put_float_by_index(msg, 12, y); // Global Y position - put_float_by_index(msg, 16, z); // Global Z position - put_float_by_index(msg, 20, roll); // Roll angle in rad - put_float_by_index(msg, 24, pitch); // Pitch angle in rad - put_float_by_index(msg, 28, yaw); // Yaw angle in rad + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; return mavlink_finalize_message(msg, system_id, component_id, 32, 158); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy mavlink_message_t* msg, uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) { - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X position - put_float_by_index(msg, 12, y); // Global Y position - put_float_by_index(msg, 16, z); // Global Z position - put_float_by_index(msg, 20, roll); // Roll angle in rad - put_float_by_index(msg, 24, pitch); // Pitch angle in rad - put_float_by_index(msg, 28, yaw); // Yaw angle in rad + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 158); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste 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_ALIGNED_MESSAGE(msg, 32); - msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 20, roll); + _mav_put_float(buf, 24, pitch); + _mav_put_float(buf, 28, yaw); - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X position - put_float_by_index(msg, 12, y); // Global Y position - put_float_by_index(msg, 16, z); // Global Z position - put_float_by_index(msg, 20, roll); // Roll angle in rad - put_float_by_index(msg, 24, pitch); // Pitch angle in rad - put_float_by_index(msg, 28, yaw); // Yaw angle in rad + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, 32, 158); +#else + mavlink_vision_position_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; - mavlink_finalize_message_chan_send(msg, chan, 32, 158); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, 32, 158); +#endif } #endif @@ -150,7 +191,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -160,7 +201,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -170,7 +211,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -180,7 +221,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -190,7 +231,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -200,7 +241,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -210,7 +251,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_mes vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); #else - memcpy(vision_position_estimate, MAVLINK_PAYLOAD(msg), 32); + memcpy(vision_position_estimate, _MAV_PAYLOAD(msg), 32); #endif } 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 4897307fa..aa75a2bf5 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -41,13 +41,25 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X speed - put_float_by_index(msg, 12, y); // Global Y speed - put_float_by_index(msg, 16, z); // Global Z speed + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; return mavlink_finalize_message(msg, system_id, component_id, 20, 208); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste mavlink_message_t* msg, uint64_t usec,float x,float y,float z) { - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + + memcpy(_MAV_PAYLOAD(msg), buf, 20); +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X speed - put_float_by_index(msg, 12, y); // Global Y speed - put_float_by_index(msg, 16, z); // Global Z speed + memcpy(_MAV_PAYLOAD(msg), &packet, 20); +#endif + msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 208); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) { - MAVLINK_ALIGNED_MESSAGE(msg, 20); - msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - - put_uint64_t_by_index(msg, 0, usec); // Timestamp (milliseconds) - put_float_by_index(msg, 8, x); // Global X speed - put_float_by_index(msg, 12, y); // Global Y speed - put_float_by_index(msg, 16, z); // Global Z speed +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[20]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, 20, 208); +#else + mavlink_vision_speed_estimate_t packet; + packet.usec = usec; + packet.x = x; + packet.y = y; + packet.z = z; - mavlink_finalize_message_chan_send(msg, chan, 20, 208); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, 20, 208); +#endif } #endif @@ -126,7 +158,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -136,7 +168,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -146,7 +178,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -156,7 +188,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_messag vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); #else - memcpy(vision_speed_estimate, MAVLINK_PAYLOAD(msg), 20); + memcpy(vision_speed_estimate, _MAV_PAYLOAD(msg), 20); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index a3e3f52be..e9a7ee127 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -41,13 +41,25 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_id); + _mav_put_uint8_t(buf, 4, target_system_id); + _mav_put_uint8_t(buf, 5, command_id); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_watchdog_command_t packet; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + packet.target_system_id = target_system_id; + packet.command_id = command_id; - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_id); // Process ID - put_uint8_t_by_index(msg, 4, target_system_id); // Target system ID - put_uint8_t_by_index(msg, 5, command_id); // Command ID + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; return mavlink_finalize_message(msg, system_id, component_id, 6, 162); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, mavlink_message_t* msg, uint8_t target_system_id,uint16_t watchdog_id,uint16_t process_id,uint8_t command_id) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_id); + _mav_put_uint8_t(buf, 4, target_system_id); + _mav_put_uint8_t(buf, 5, command_id); + + memcpy(_MAV_PAYLOAD(msg), buf, 6); +#else + mavlink_watchdog_command_t packet; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + packet.target_system_id = target_system_id; + packet.command_id = command_id; - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_id); // Process ID - put_uint8_t_by_index(msg, 4, target_system_id); // Target system ID - put_uint8_t_by_index(msg, 5, command_id); // Command ID + memcpy(_MAV_PAYLOAD(msg), &packet, 6); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 162); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui 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_ALIGNED_MESSAGE(msg, 6); - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_id); // Process ID - put_uint8_t_by_index(msg, 4, target_system_id); // Target system ID - put_uint8_t_by_index(msg, 5, command_id); // Command ID +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[6]; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_id); + _mav_put_uint8_t(buf, 4, target_system_id); + _mav_put_uint8_t(buf, 5, command_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, buf, 6, 162); +#else + mavlink_watchdog_command_t packet; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + packet.target_system_id = target_system_id; + packet.command_id = command_id; - mavlink_finalize_message_chan_send(msg, chan, 6, 162); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND, (const char *)&packet, 6, 162); +#endif } #endif @@ -126,7 +158,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -136,7 +168,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -146,7 +178,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 2); + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -156,7 +188,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_watchdog_command_decode(const mavlink_message_t* watchdog_command->target_system_id = mavlink_msg_watchdog_command_get_target_system_id(msg); watchdog_command->command_id = mavlink_msg_watchdog_command_get_command_id(msg); #else - memcpy(watchdog_command, MAVLINK_PAYLOAD(msg), 6); + memcpy(watchdog_command, _MAV_PAYLOAD(msg), 6); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index fd51415c9..6922e0c69 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -35,11 +35,21 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_count); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_watchdog_heartbeat_t packet; + packet.watchdog_id = watchdog_id; + packet.process_count = process_count; - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_count); // Number of processes + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; return mavlink_finalize_message(msg, system_id, component_id, 4, 153); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i mavlink_message_t* msg, uint16_t watchdog_id,uint16_t process_count) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_count); - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_count); // Number of processes + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_watchdog_heartbeat_t packet; + packet.watchdog_id = watchdog_id; + packet.process_count = process_count; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 153); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, watchdog_id); + _mav_put_uint16_t(buf, 2, process_count); - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_count); // Number of processes + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, buf, 4, 153); +#else + mavlink_watchdog_heartbeat_t packet; + packet.watchdog_id = watchdog_id; + packet.process_count = process_count; - mavlink_finalize_message_chan_send(msg, chan, 4, 153); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT, (const char *)&packet, 4, 153); +#endif } #endif @@ -110,7 +136,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -120,7 +146,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 2); + return _MAV_RETURN_uint16_t(msg, 2); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_watchdog_heartbeat_decode(const mavlink_message_t watchdog_heartbeat->watchdog_id = mavlink_msg_watchdog_heartbeat_get_watchdog_id(msg); watchdog_heartbeat->process_count = mavlink_msg_watchdog_heartbeat_get_process_count(msg); #else - memcpy(watchdog_heartbeat, MAVLINK_PAYLOAD(msg), 4); + memcpy(watchdog_heartbeat, _MAV_PAYLOAD(msg), 4); #endif } 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 4dc458166..de2d66e61 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -45,14 +45,25 @@ typedef struct __mavlink_watchdog_process_info_t 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) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - - put_int32_t_by_index(msg, 0, timeout); // Timeout (seconds) - put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 6, process_id); // Process ID - put_char_array_by_index(msg, 8, name, 100); // Process name - put_char_array_by_index(msg, 108, arguments, 147); // Process arguments +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[255]; + _mav_put_int32_t(buf, 0, timeout); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_char_array(buf, 8, name, 100); + _mav_put_char_array(buf, 108, arguments, 147); + memcpy(_MAV_PAYLOAD(msg), buf, 255); +#else + mavlink_watchdog_process_info_t packet; + packet.timeout = timeout; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + memcpy(packet.name, name, sizeof(char)*100); + memcpy(packet.arguments, arguments, sizeof(char)*147); + memcpy(_MAV_PAYLOAD(msg), &packet, 255); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; return mavlink_finalize_message(msg, system_id, component_id, 255, 16); } @@ -73,14 +84,25 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste mavlink_message_t* msg, uint16_t watchdog_id,uint16_t process_id,const char *name,const char *arguments,int32_t timeout) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - - put_int32_t_by_index(msg, 0, timeout); // Timeout (seconds) - put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 6, process_id); // Process ID - put_char_array_by_index(msg, 8, name, 100); // Process name - put_char_array_by_index(msg, 108, arguments, 147); // Process arguments +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[255]; + _mav_put_int32_t(buf, 0, timeout); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_char_array(buf, 8, name, 100); + _mav_put_char_array(buf, 108, arguments, 147); + memcpy(_MAV_PAYLOAD(msg), buf, 255); +#else + mavlink_watchdog_process_info_t packet; + packet.timeout = timeout; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + memcpy(packet.name, name, sizeof(char)*100); + memcpy(packet.arguments, arguments, sizeof(char)*147); + memcpy(_MAV_PAYLOAD(msg), &packet, 255); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 16); } @@ -111,16 +133,23 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i 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_ALIGNED_MESSAGE(msg, 255); - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - - put_int32_t_by_index(msg, 0, timeout); // Timeout (seconds) - put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 6, process_id); // Process ID - put_char_array_by_index(msg, 8, name, 100); // Process name - put_char_array_by_index(msg, 108, arguments, 147); // Process arguments - - mavlink_finalize_message_chan_send(msg, chan, 255, 16); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[255]; + _mav_put_int32_t(buf, 0, timeout); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_char_array(buf, 8, name, 100); + _mav_put_char_array(buf, 108, arguments, 147); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, buf, 255, 16); +#else + mavlink_watchdog_process_info_t packet; + packet.timeout = timeout; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + memcpy(packet.name, name, sizeof(char)*100); + memcpy(packet.arguments, arguments, sizeof(char)*147); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO, (const char *)&packet, 255, 16); +#endif } #endif @@ -135,7 +164,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -145,7 +174,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -155,7 +184,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const ma */ static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char *name) { - return MAVLINK_MSG_RETURN_char_array(msg, name, 100, 8); + return _MAV_RETURN_char_array(msg, name, 100, 8); } /** @@ -165,7 +194,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_ */ static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char *arguments) { - return MAVLINK_MSG_RETURN_char_array(msg, arguments, 147, 108); + return _MAV_RETURN_char_array(msg, arguments, 147, 108); } /** @@ -175,7 +204,7 @@ 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) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -193,6 +222,6 @@ static inline void mavlink_msg_watchdog_process_info_decode(const mavlink_messag mavlink_msg_watchdog_process_info_get_name(msg, watchdog_process_info->name); mavlink_msg_watchdog_process_info_get_arguments(msg, watchdog_process_info->arguments); #else - memcpy(watchdog_process_info, MAVLINK_PAYLOAD(msg), 255); + memcpy(watchdog_process_info, _MAV_PAYLOAD(msg), 255); #endif } 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 fbe32361f..77f730029 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - - put_int32_t_by_index(msg, 0, pid); // PID - put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 6, process_id); // Process ID - put_uint16_t_by_index(msg, 8, crashes); // Number of crashes - put_uint8_t_by_index(msg, 10, state); // Is running / finished / suspended / crashed - put_uint8_t_by_index(msg, 11, muted); // Is muted +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, pid); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_uint16_t(buf, 8, crashes); + _mav_put_uint8_t(buf, 10, state); + _mav_put_uint8_t(buf, 11, muted); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_watchdog_process_status_t packet; + packet.pid = pid; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + packet.crashes = crashes; + packet.state = state; + packet.muted = muted; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; return mavlink_finalize_message(msg, system_id, component_id, 12, 29); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys mavlink_message_t* msg, uint16_t watchdog_id,uint16_t process_id,uint8_t state,uint8_t muted,int32_t pid,uint16_t crashes) { - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - - put_int32_t_by_index(msg, 0, pid); // PID - put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 6, process_id); // Process ID - put_uint16_t_by_index(msg, 8, crashes); // Number of crashes - put_uint8_t_by_index(msg, 10, state); // Is running / finished / suspended / crashed - put_uint8_t_by_index(msg, 11, muted); // Is muted +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, pid); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_uint16_t(buf, 8, crashes); + _mav_put_uint8_t(buf, 10, state); + _mav_put_uint8_t(buf, 11, muted); + + memcpy(_MAV_PAYLOAD(msg), buf, 12); +#else + mavlink_watchdog_process_status_t packet; + packet.pid = pid; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + packet.crashes = crashes; + packet.state = state; + packet.muted = muted; + + memcpy(_MAV_PAYLOAD(msg), &packet, 12); +#endif + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 29); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system 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_ALIGNED_MESSAGE(msg, 12); - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - - put_int32_t_by_index(msg, 0, pid); // PID - put_uint16_t_by_index(msg, 4, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 6, process_id); // Process ID - put_uint16_t_by_index(msg, 8, crashes); // Number of crashes - put_uint8_t_by_index(msg, 10, state); // Is running / finished / suspended / crashed - put_uint8_t_by_index(msg, 11, muted); // Is muted - - mavlink_finalize_message_chan_send(msg, chan, 12, 29); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[12]; + _mav_put_int32_t(buf, 0, pid); + _mav_put_uint16_t(buf, 4, watchdog_id); + _mav_put_uint16_t(buf, 6, process_id); + _mav_put_uint16_t(buf, 8, crashes); + _mav_put_uint8_t(buf, 10, state); + _mav_put_uint8_t(buf, 11, muted); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, buf, 12, 29); +#else + mavlink_watchdog_process_status_t packet; + packet.pid = pid; + packet.watchdog_id = watchdog_id; + packet.process_id = process_id; + packet.crashes = crashes; + packet.state = state; + packet.muted = muted; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS, (const char *)&packet, 12, 29); +#endif } #endif @@ -142,7 +180,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 4); + return _MAV_RETURN_uint16_t(msg, 4); } /** @@ -152,7 +190,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 6); + return _MAV_RETURN_uint16_t(msg, 6); } /** @@ -162,7 +200,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 10); + return _MAV_RETURN_uint8_t(msg, 10); } /** @@ -172,7 +210,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 11); + return _MAV_RETURN_uint8_t(msg, 11); } /** @@ -182,7 +220,7 @@ 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) { - return MAVLINK_MSG_RETURN_int32_t(msg, 0); + return _MAV_RETURN_int32_t(msg, 0); } /** @@ -192,7 +230,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_watchdog_process_status_decode(const mavlink_mess watchdog_process_status->state = mavlink_msg_watchdog_process_status_get_state(msg); watchdog_process_status->muted = mavlink_msg_watchdog_process_status_get_muted(msg); #else - memcpy(watchdog_process_status, MAVLINK_PAYLOAD(msg), 12); + memcpy(watchdog_process_status, _MAV_PAYLOAD(msg), 12); #endif } diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 2dad8e300..4fef625c3 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -12,15 +12,19 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {7, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 101, 22, 26, 16, 14, 28, 28, 24, 0, 22, 22, 21, 0, 36, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 18, 16, 14, 14, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 32, 32, 20, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 8, 255, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 3} +#define MAVLINK_MESSAGE_LENGTHS {7, 33, 12, 0, 14, 28, 3, 32, 0, 0, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 0, 30, 101, 22, 26, 16, 14, 28, 28, 24, 0, 22, 22, 21, 0, 36, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 18, 16, 14, 14, 27, 25, 18, 18, 20, 20, 0, 0, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 20, 32, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 32, 32, 20, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 26, 16, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 0, 0, 0, 0, 0, 0, 106, 43, 55, 8, 255, 53, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 3} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {88, 236, 137, 0, 237, 217, 104, 119, 0, 0, 0, 197, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 24, 23, 170, 144, 67, 115, 39, 231, 102, 0, 244, 237, 222, 0, 205, 51, 80, 101, 213, 8, 229, 21, 214, 41, 39, 131, 50, 142, 53, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 108, 86, 95, 224, 0, 158, 56, 208, 11, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 148, 223, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247} +#define MAVLINK_MESSAGE_CRCS {88, 28, 137, 0, 237, 217, 104, 119, 0, 0, 0, 197, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 0, 24, 23, 170, 144, 67, 115, 39, 231, 102, 0, 244, 237, 222, 0, 205, 51, 80, 101, 213, 8, 229, 21, 214, 41, 39, 131, 50, 142, 53, 15, 3, 100, 24, 239, 238, 0, 0, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 160, 168, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 19, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 108, 86, 95, 224, 0, 158, 56, 208, 11, 244, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 249, 182, 0, 0, 0, 0, 0, 0, 0, 153, 16, 29, 162, 0, 0, 0, 0, 0, 0, 90, 95, 36, 148, 223, 88, 0, 0, 0, 0, 254, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 247} #endif #ifndef MAVLINK_MESSAGE_INFO +<<<<<<< HEAD #define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT_SET, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_OFFSET_SET, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_EXTENDED_MESSAGE} +======= +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {NULL}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_CAM_SHUTTER, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGERED, MAVLINK_MESSAGE_INFO_IMAGE_TRIGGER_CONTROL, MAVLINK_MESSAGE_INFO_IMAGE_AVAILABLE, {NULL}, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT_SET, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_OFFSET_SET, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_POSITION_CONTROL_SETPOINT, MAVLINK_MESSAGE_INFO_MARKER, MAVLINK_MESSAGE_INFO_RAW_AUX, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_WATCHDOG_HEARTBEAT, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_INFO, MAVLINK_MESSAGE_INFO_WATCHDOG_PROCESS_STATUS, MAVLINK_MESSAGE_INFO_WATCHDOG_COMMAND, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PATTERN_DETECTED, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST, MAVLINK_MESSAGE_INFO_POINT_OF_INTEREST_CONNECTION, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_BRIEF_FEATURE, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_ATTITUDE_CONTROL, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +>>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/pixhawk/testsuite.h b/thirdParty/mavlink/include/pixhawk/testsuite.h index fad934f34..d24f7d6f3 100644 --- a/thirdParty/mavlink/include/pixhawk/testsuite.h +++ b/thirdParty/mavlink/include/pixhawk/testsuite.h @@ -11,25 +11,25 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t); -static void mavlink_test_pixhawk(uint8_t, uint8_t); +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_pixhawk(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_common(system_id, component_id); - mavlink_test_pixhawk(system_id, component_id); + mavlink_test_common(system_id, component_id, last_msg); + mavlink_test_pixhawk(system_id, component_id, last_msg); } #endif #include "../common/testsuite.h" -static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id) +static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_set_cam_shutter_t packet2, packet1 = { + mavlink_set_cam_shutter_t packet_in = { 17.0, 17443, 17547, @@ -37,27 +37,52 @@ static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id 96, 163, }; - if (sizeof(packet2) != 11) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_set_cam_shutter_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.gain = packet_in.gain; + packet1.interval = packet_in.interval; + packet1.exposure = packet_in.exposure; + packet1.cam_no = packet_in.cam_no; + packet1.cam_mode = packet_in.cam_mode; + packet1.trigger_pin = packet_in.trigger_pin; + + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_set_cam_shutter_encode(system_id, component_id, &msg, &packet1); mavlink_msg_set_cam_shutter_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_set_cam_shutter_pack(system_id, component_id, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); + mavlink_msg_set_cam_shutter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_set_cam_shutter_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.cam_no , packet1.cam_mode , packet1.trigger_pin , packet1.interval , packet1.exposure , packet1.gain ); + mavlink_msg_set_cam_shutter_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #include "mavlink.h" diff --git a/thirdParty/mavlink/include/protocol.h b/thirdParty/mavlink/include/protocol.h index b218ece8f..7b306d2a9 100644 --- a/thirdParty/mavlink/include/protocol.h +++ b/thirdParty/mavlink/include/protocol.h @@ -18,10 +18,22 @@ #define MAVLINK_STACK_BUFFER 0 #endif +#ifndef MAVLINK_AVOID_GCC_STACK_BUG +# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__) +#endif + #ifndef MAVLINK_ASSERT #define MAVLINK_ASSERT(x) #endif +#ifndef MAVLINK_START_UART_SEND +#define MAVLINK_START_UART_SEND(chan, length) +#endif + +#ifndef MAVLINK_END_UART_SEND +#define MAVLINK_END_UART_SEND(chan, length) +#endif + #ifdef MAVLINK_SEPARATE_HELPERS #define MAVLINK_HELPER #else @@ -36,19 +48,14 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui uint8_t chan, uint16_t length, uint8_t crc_extra); MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length, uint8_t crc_extra); -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void mavlink_finalize_message_chan_send(mavlink_message_t* msg, - mavlink_channel_t chan, uint16_t length, uint8_t crc_extra); -#endif +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, + uint8_t length, uint8_t crc_extra); #else MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length); MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length); -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void mavlink_finalize_message_chan_send(mavlink_message_t* msg, - mavlink_channel_t chan, uint16_t length); -#endif +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length); #endif // MAVLINK_CRC_EXTRA MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg); MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg); @@ -57,7 +64,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer); #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -MAVLINK_HELPER void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); #endif /** @@ -69,19 +76,19 @@ static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_ } #if MAVLINK_NEED_BYTE_SWAP -static inline void byte_swap_2(uint8_t *dst, const uint8_t *src) +static inline void byte_swap_2(char *dst, const char *src) { dst[0] = src[1]; dst[1] = src[0]; } -static inline void byte_swap_4(uint8_t *dst, const uint8_t *src) +static inline void byte_swap_4(char *dst, const char *src) { dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; } -static inline void byte_swap_8(uint8_t *dst, const uint8_t *src) +static inline void byte_swap_8(char *dst, const char *src) { dst[0] = src[7]; dst[1] = src[6]; @@ -93,238 +100,223 @@ static inline void byte_swap_8(uint8_t *dst, const uint8_t *src) dst[7] = src[0]; } #elif !MAVLINK_ALIGNED_FIELDS -static inline void byte_copy_2(uint8_t *dst, const uint8_t *src) +static inline void byte_copy_2(char *dst, const char *src) { dst[0] = src[0]; dst[1] = src[1]; } -static inline void byte_copy_4(uint8_t *dst, const uint8_t *src) +static inline void byte_copy_4(char *dst, const char *src) { dst[0] = src[0]; dst[1] = src[1]; dst[2] = src[2]; dst[3] = src[3]; } -static inline void byte_copy_8(uint8_t *dst, const uint8_t *src) +static inline void byte_copy_8(char *dst, const char *src) { memcpy(dst, src, 8); } #endif -#define put_uint8_t_by_index(msg, wire_offset, b) msg->payload.u8[wire_offset] = b -#define put_int8_t_by_index(msg, wire_offset, b) msg->payload.i8[wire_offset] = b -#define put_char_by_index(msg, wire_offset, b) msg->payload.c[wire_offset] = b +#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b +#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b +#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b #if MAVLINK_NEED_BYTE_SWAP -#define put_uint16_t_by_index(msg, wire_offset, b) byte_swap_2(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_int16_t_by_index(msg, wire_offset, b) byte_swap_2(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_uint32_t_by_index(msg, wire_offset, b) byte_swap_4(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_int32_t_by_index(msg, wire_offset, b) byte_swap_4(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_uint64_t_by_index(msg, wire_offset, b) byte_swap_8(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_int64_t_by_index(msg, wire_offset, b) byte_swap_8(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_float_by_index(msg, wire_offset, b) byte_swap_4(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_double_by_index(msg, wire_offset, b) byte_swap_8(&msg->payload.u8[wire_offset], (const uint8_t *)&b) +#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b) #elif !MAVLINK_ALIGNED_FIELDS -#define put_uint16_t_by_index(msg, wire_offset, b) byte_copy_2(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_int16_t_by_index(msg, wire_offset, b) byte_copy_2(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_uint32_t_by_index(msg, wire_offset, b) byte_copy_4(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_int32_t_by_index(msg, wire_offset, b) byte_copy_4(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_uint64_t_by_index(msg, wire_offset, b) byte_copy_8(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_int64_t_by_index(msg, wire_offset, b) byte_copy_8(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_float_by_index(msg, wire_offset, b) byte_copy_4(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#define put_double_by_index(msg, wire_offset, b) byte_copy_8(&msg->payload.u8[wire_offset], (const uint8_t *)&b) -#else // nicely aligned, no swap -#define put_uint16_t_by_index(msg, wire_offset, b) msg->payload.u16[wire_offset/2] = b -#define put_int16_t_by_index(msg, wire_offset, b) msg->payload.i16[wire_offset/2] = b -#define put_uint32_t_by_index(msg, wire_offset, b) msg->payload.u32[wire_offset/4] = b -#define put_int32_t_by_index(msg, wire_offset, b) msg->payload.i32[wire_offset/4] = b -#define put_uint64_t_by_index(msg, wire_offset, b) msg->payload.u64[wire_offset/8] = b -#define put_int64_t_by_index(msg, wire_offset, b) msg->payload.i64[wire_offset/8] = b -#define put_float_by_index(msg, wire_offset, b) msg->payload.f[wire_offset/4] = b -#define put_double_by_index(msg, wire_offset, b) msg->payload.d[wire_offset/8] = b +#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b) +#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b) +#else +#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b +#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b +#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b +#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b +#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b +#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b +#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b +#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b #endif /* * Place a char array into a buffer */ -static inline void put_char_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const char *b, uint8_t array_length) +static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) { if (b == NULL) { - memset(&msg->payload.c[wire_offset], 0, array_length); + memset(&buf[wire_offset], 0, array_length); } else { - memcpy(&msg->payload.c[wire_offset], b, array_length); + memcpy(&buf[wire_offset], b, array_length); } } /* * Place a uint8_t array into a buffer */ -static inline void put_uint8_t_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) +static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) { if (b == NULL) { - memset(&msg->payload.u8[wire_offset], 0, array_length); + memset(&buf[wire_offset], 0, array_length); } else { - memcpy(&msg->payload.u8[wire_offset], b, array_length); + memcpy(&buf[wire_offset], b, array_length); } } /* * Place a int8_t array into a buffer */ -static inline void put_int8_t_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const int8_t *b, uint8_t array_length) +static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) { if (b == NULL) { - memset(&msg->payload.i8[wire_offset], 0, array_length); + memset(&buf[wire_offset], 0, array_length); } else { - memcpy(&msg->payload.i8[wire_offset], b, array_length); + memcpy(&buf[wire_offset], b, array_length); } } #if MAVLINK_NEED_BYTE_SWAP -#define PUT_ARRAY_BY_INDEX(TYPE, V) \ -static inline void put_ ## TYPE ##_array_by_index(mavlink_message_t *msg, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +#define _MAV_PUT_ARRAY(TYPE, V) \ +static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ { \ if (b == NULL) { \ - memset(&msg->payload.u8[wire_offset], 0, array_length*sizeof(TYPE)); \ + memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ } else { \ uint16_t i; \ for (i=0; ipayload.u8[wire_offset], 0, array_length*sizeof(TYPE)); \ + memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ } else { \ - memcpy(&msg->payload.V[wire_offset/sizeof(TYPE)], b, array_length*sizeof(TYPE)); \ + memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \ } \ } #endif -PUT_ARRAY_BY_INDEX(uint16_t, u16) -PUT_ARRAY_BY_INDEX(uint32_t, u32) -PUT_ARRAY_BY_INDEX(uint64_t, u64) -PUT_ARRAY_BY_INDEX(int16_t, i16) -PUT_ARRAY_BY_INDEX(int32_t, i32) -PUT_ARRAY_BY_INDEX(int64_t, i64) -PUT_ARRAY_BY_INDEX(float, f) -PUT_ARRAY_BY_INDEX(double, d) +_MAV_PUT_ARRAY(uint16_t, u16) +_MAV_PUT_ARRAY(uint32_t, u32) +_MAV_PUT_ARRAY(uint64_t, u64) +_MAV_PUT_ARRAY(int16_t, i16) +_MAV_PUT_ARRAY(int32_t, i32) +_MAV_PUT_ARRAY(int64_t, i64) +_MAV_PUT_ARRAY(float, f) +_MAV_PUT_ARRAY(double, d) -#define MAVLINK_MSG_RETURN_char(msg, wire_offset) msg->payload.c[wire_offset] -#define MAVLINK_MSG_RETURN_int8_t(msg, wire_offset) msg->payload.i8[wire_offset] -#define MAVLINK_MSG_RETURN_uint8_t(msg, wire_offset) msg->payload.u8[wire_offset] +#define _MAV_RETURN_char(msg, wire_offset) _MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_int8_t(msg, wire_offset) (int8_t)_MAV_PAYLOAD(msg)[wire_offset] +#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset] #if MAVLINK_NEED_BYTE_SWAP -#define MAVLINK_MSG_RETURN_TYPE(TYPE, SIZE) \ -static inline TYPE MAVLINK_MSG_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ -{ TYPE r; byte_swap_## SIZE((uint8_t*)&r, &msg->payload.u8[ofs]); return r; } +#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ +static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; } -MAVLINK_MSG_RETURN_TYPE(uint16_t, 2) -MAVLINK_MSG_RETURN_TYPE(int16_t, 2) -MAVLINK_MSG_RETURN_TYPE(uint32_t, 4) -MAVLINK_MSG_RETURN_TYPE(int32_t, 4) -MAVLINK_MSG_RETURN_TYPE(uint64_t, 8) -MAVLINK_MSG_RETURN_TYPE(int64_t, 8) -MAVLINK_MSG_RETURN_TYPE(float, 4) -MAVLINK_MSG_RETURN_TYPE(double, 8) +_MAV_MSG_RETURN_TYPE(uint16_t, 2) +_MAV_MSG_RETURN_TYPE(int16_t, 2) +_MAV_MSG_RETURN_TYPE(uint32_t, 4) +_MAV_MSG_RETURN_TYPE(int32_t, 4) +_MAV_MSG_RETURN_TYPE(uint64_t, 8) +_MAV_MSG_RETURN_TYPE(int64_t, 8) +_MAV_MSG_RETURN_TYPE(float, 4) +_MAV_MSG_RETURN_TYPE(double, 8) #elif !MAVLINK_ALIGNED_FIELDS -#define MAVLINK_MSG_RETURN_TYPE(TYPE, SIZE) \ -static inline TYPE MAVLINK_MSG_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ -{ TYPE r; byte_copy_## SIZE((uint8_t*)&r, &msg->payload.u8[ofs]); return r; } +#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ +static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; } -MAVLINK_MSG_RETURN_TYPE(uint16_t, 2) -MAVLINK_MSG_RETURN_TYPE(int16_t, 2) -MAVLINK_MSG_RETURN_TYPE(uint32_t, 4) -MAVLINK_MSG_RETURN_TYPE(int32_t, 4) -MAVLINK_MSG_RETURN_TYPE(uint64_t, 8) -MAVLINK_MSG_RETURN_TYPE(int64_t, 8) -MAVLINK_MSG_RETURN_TYPE(float, 4) -MAVLINK_MSG_RETURN_TYPE(double, 8) +_MAV_MSG_RETURN_TYPE(uint16_t, 2) +_MAV_MSG_RETURN_TYPE(int16_t, 2) +_MAV_MSG_RETURN_TYPE(uint32_t, 4) +_MAV_MSG_RETURN_TYPE(int32_t, 4) +_MAV_MSG_RETURN_TYPE(uint64_t, 8) +_MAV_MSG_RETURN_TYPE(int64_t, 8) +_MAV_MSG_RETURN_TYPE(float, 4) +_MAV_MSG_RETURN_TYPE(double, 8) +#else // nicely aligned, no swap +#define _MAV_MSG_RETURN_TYPE(TYPE) \ +static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);} -#else // no swap, nicely aligned -#define MAVLINK_MSG_RETURN_uint16_t(msg, ofs) msg->payload.u16[ofs/2] -#define MAVLINK_MSG_RETURN_int16_t(msg, ofs) msg->payload.i16[ofs/2] -#define MAVLINK_MSG_RETURN_uint32_t(msg, ofs) msg->payload.u32[ofs/4] -#define MAVLINK_MSG_RETURN_int32_t(msg, ofs) msg->payload.i32[ofs/4] -#define MAVLINK_MSG_RETURN_uint64_t(msg, ofs) msg->payload.u64[ofs/8] -#define MAVLINK_MSG_RETURN_int64_t(msg, ofs) msg->payload.i64[ofs/8] -#define MAVLINK_MSG_RETURN_float(msg, ofs) msg->payload.f[ofs/4] -#define MAVLINK_MSG_RETURN_double(msg, ofs) msg->payload.d[ofs/8] +_MAV_MSG_RETURN_TYPE(uint16_t) +_MAV_MSG_RETURN_TYPE(int16_t) +_MAV_MSG_RETURN_TYPE(uint32_t) +_MAV_MSG_RETURN_TYPE(int32_t) +_MAV_MSG_RETURN_TYPE(uint64_t) +_MAV_MSG_RETURN_TYPE(int64_t) +_MAV_MSG_RETURN_TYPE(float) +_MAV_MSG_RETURN_TYPE(double) #endif // MAVLINK_NEED_BYTE_SWAP -static inline uint16_t MAVLINK_MSG_RETURN_char_array(const mavlink_message_t *msg, char *value, +static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, uint8_t array_length, uint8_t wire_offset) { - memcpy(value, &msg->payload.c[wire_offset], array_length); + memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } -static inline uint16_t MAVLINK_MSG_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, +static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, uint8_t array_length, uint8_t wire_offset) { - memcpy(value, &msg->payload.u8[wire_offset], array_length); + memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } -static inline uint16_t MAVLINK_MSG_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, +static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, uint8_t array_length, uint8_t wire_offset) { - memcpy(value, &msg->payload.i8[wire_offset], array_length); + memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } #if MAVLINK_NEED_BYTE_SWAP -#define RETURN_ARRAY_BY_INDEX(TYPE, V) \ -static inline uint16_t MAVLINK_MSG_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ +#define _MAV_RETURN_ARRAY(TYPE, V) \ +static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ uint8_t array_length, uint8_t wire_offset) \ { \ uint16_t i; \ for (i=0; ipayload.V[wire_offset/sizeof(TYPE)], array_length*sizeof(TYPE)); \ + memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \ return array_length*sizeof(TYPE); \ } #endif -RETURN_ARRAY_BY_INDEX(uint16_t, u16) -RETURN_ARRAY_BY_INDEX(uint32_t, u32) -RETURN_ARRAY_BY_INDEX(uint64_t, u64) -RETURN_ARRAY_BY_INDEX(int16_t, i16) -RETURN_ARRAY_BY_INDEX(int32_t, i32) -RETURN_ARRAY_BY_INDEX(int64_t, i64) -RETURN_ARRAY_BY_INDEX(float, f) -RETURN_ARRAY_BY_INDEX(double, d) - -#if MAVLINK_STACK_BUFFER == 1 -/* - we need to be able to declare a mavlink_message_t on the stack for - _send() calls. This can either be done by declaring a whole message, - or by declaring a buffer of just the right size for this specific - message type, and casting a mavlink_message_t structure to it. Using - the cast is a violation of the strict aliasing rules for C, so only - use it if you know its OK for your architecture - */ -#define MAVLINK_ALIGNED_MESSAGE(msg, length) \ -uint64_t _buf[(MAVLINK_NUM_CHECKSUM_BYTES+MAVLINK_NUM_NON_PAYLOAD_BYTES+(length)+7)/8]; \ -mavlink_message_t *msg = (mavlink_message_t *)&_buf[0] -#else -#define MAVLINK_ALIGNED_MESSAGE(msg, length) \ - mavlink_message_t _msg; \ - mavlink_message_t *msg = &_msg -#endif // MAVLINK_STACK_BUFFER +_MAV_RETURN_ARRAY(uint16_t, u16) +_MAV_RETURN_ARRAY(uint32_t, u32) +_MAV_RETURN_ARRAY(uint64_t, u64) +_MAV_RETURN_ARRAY(int16_t, i16) +_MAV_RETURN_ARRAY(int32_t, i32) +_MAV_RETURN_ARRAY(int64_t, i64) +_MAV_RETURN_ARRAY(float, f) +_MAV_RETURN_ARRAY(double, d) #endif // _MAVLINK_PROTOCOL_H_ diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index fc8714581..ee2eceda2 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_float(buf, 0, dynamicPressure); + _mav_put_float(buf, 4, staticPressure); + _mav_put_uint16_t(buf, 8, temperature); + + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_air_data_t packet; + packet.dynamicPressure = dynamicPressure; + packet.staticPressure = staticPressure; + packet.temperature = temperature; - put_float_by_index(msg, 0, dynamicPressure); // Dynamic pressure (Pa) - put_float_by_index(msg, 4, staticPressure); // Static pressure (Pa) - put_uint16_t_by_index(msg, 8, temperature); // Board temperature + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + msg->msgid = MAVLINK_MSG_ID_AIR_DATA; return mavlink_finalize_message(msg, system_id, component_id, 10, 232); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, float dynamicPressure,float staticPressure,uint16_t temperature) { - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_float(buf, 0, dynamicPressure); + _mav_put_float(buf, 4, staticPressure); + _mav_put_uint16_t(buf, 8, temperature); - put_float_by_index(msg, 0, dynamicPressure); // Dynamic pressure (Pa) - put_float_by_index(msg, 4, staticPressure); // Static pressure (Pa) - put_uint16_t_by_index(msg, 8, temperature); // Board temperature + memcpy(_MAV_PAYLOAD(msg), buf, 10); +#else + mavlink_air_data_t packet; + packet.dynamicPressure = dynamicPressure; + packet.staticPressure = staticPressure; + packet.temperature = temperature; + memcpy(_MAV_PAYLOAD(msg), &packet, 10); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIR_DATA; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 232); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) { - MAVLINK_ALIGNED_MESSAGE(msg, 10); - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[10]; + _mav_put_float(buf, 0, dynamicPressure); + _mav_put_float(buf, 4, staticPressure); + _mav_put_uint16_t(buf, 8, temperature); - put_float_by_index(msg, 0, dynamicPressure); // Dynamic pressure (Pa) - put_float_by_index(msg, 4, staticPressure); // Static pressure (Pa) - put_uint16_t_by_index(msg, 8, temperature); // Board temperature + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIR_DATA, buf, 10, 232); +#else + mavlink_air_data_t packet; + packet.dynamicPressure = dynamicPressure; + packet.staticPressure = staticPressure; + packet.temperature = temperature; - mavlink_finalize_message_chan_send(msg, chan, 10, 232); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIR_DATA, (const char *)&packet, 10, 232); +#endif } #endif @@ -118,7 +147,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -128,7 +157,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -138,7 +167,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 8); + return _MAV_RETURN_uint16_t(msg, 8); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_air_data_decode(const mavlink_message_t* msg, mav air_data->staticPressure = mavlink_msg_air_data_get_staticPressure(msg); air_data->temperature = mavlink_msg_air_data_get_temperature(msg); #else - memcpy(air_data, MAVLINK_PAYLOAD(msg), 10); + memcpy(air_data, _MAV_PAYLOAD(msg), 10); #endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index e14f1e824..5c4719552 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, batVolt); + _mav_put_uint8_t(buf, 2, sensLoad); + _mav_put_uint8_t(buf, 3, ctrlLoad); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_cpu_load_t packet; + packet.batVolt = batVolt; + packet.sensLoad = sensLoad; + packet.ctrlLoad = ctrlLoad; - put_uint16_t_by_index(msg, 0, batVolt); // Battery Voltage in millivolts - put_uint8_t_by_index(msg, 2, sensLoad); // Sensor DSC Load - put_uint8_t_by_index(msg, 3, ctrlLoad); // Control DSC Load + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; return mavlink_finalize_message(msg, system_id, component_id, 4, 75); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, uint8_t sensLoad,uint8_t ctrlLoad,uint16_t batVolt) { - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, batVolt); + _mav_put_uint8_t(buf, 2, sensLoad); + _mav_put_uint8_t(buf, 3, ctrlLoad); - put_uint16_t_by_index(msg, 0, batVolt); // Battery Voltage in millivolts - put_uint8_t_by_index(msg, 2, sensLoad); // Sensor DSC Load - put_uint8_t_by_index(msg, 3, ctrlLoad); // Control DSC Load + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_cpu_load_t packet; + packet.batVolt = batVolt; + packet.sensLoad = sensLoad; + packet.ctrlLoad = ctrlLoad; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 75); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, batVolt); + _mav_put_uint8_t(buf, 2, sensLoad); + _mav_put_uint8_t(buf, 3, ctrlLoad); - put_uint16_t_by_index(msg, 0, batVolt); // Battery Voltage in millivolts - put_uint8_t_by_index(msg, 2, sensLoad); // Sensor DSC Load - put_uint8_t_by_index(msg, 3, ctrlLoad); // Control DSC Load + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CPU_LOAD, buf, 4, 75); +#else + mavlink_cpu_load_t packet; + packet.batVolt = batVolt; + packet.sensLoad = sensLoad; + packet.ctrlLoad = ctrlLoad; - mavlink_finalize_message_chan_send(msg, chan, 4, 75); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CPU_LOAD, (const char *)&packet, 4, 75); +#endif } #endif @@ -118,7 +147,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -138,7 +167,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_cpu_load_decode(const mavlink_message_t* msg, mav cpu_load->sensLoad = mavlink_msg_cpu_load_get_sensLoad(msg); cpu_load->ctrlLoad = mavlink_msg_cpu_load_get_ctrlLoad(msg); #else - memcpy(cpu_load, MAVLINK_PAYLOAD(msg), 4); + memcpy(cpu_load, _MAV_PAYLOAD(msg), 4); #endif } 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 ae4fc86fb..128680f07 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -35,11 +35,21 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint16_t(buf, 0, bitfieldPt); + _mav_put_uint8_t(buf, 2, target); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_ctrl_srfc_pt_t packet; + packet.bitfieldPt = bitfieldPt; + packet.target = target; - put_uint16_t_by_index(msg, 0, bitfieldPt); // Bitfield containing the PT configuration - put_uint8_t_by_index(msg, 2, target); // The system setting the commands + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; return mavlink_finalize_message(msg, system_id, component_id, 3, 104); } @@ -57,11 +67,21 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint8_t target,uint16_t bitfieldPt) { - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint16_t(buf, 0, bitfieldPt); + _mav_put_uint8_t(buf, 2, target); - put_uint16_t_by_index(msg, 0, bitfieldPt); // Bitfield containing the PT configuration - put_uint8_t_by_index(msg, 2, target); // The system setting the commands + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_ctrl_srfc_pt_t packet; + packet.bitfieldPt = bitfieldPt; + packet.target = target; + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); } @@ -89,13 +109,19 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) { - MAVLINK_ALIGNED_MESSAGE(msg, 3); - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint16_t(buf, 0, bitfieldPt); + _mav_put_uint8_t(buf, 2, target); - put_uint16_t_by_index(msg, 0, bitfieldPt); // Bitfield containing the PT configuration - put_uint8_t_by_index(msg, 2, target); // The system setting the commands + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CTRL_SRFC_PT, buf, 3, 104); +#else + mavlink_ctrl_srfc_pt_t packet; + packet.bitfieldPt = bitfieldPt; + packet.target = target; - mavlink_finalize_message_chan_send(msg, chan, 3, 104); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CTRL_SRFC_PT, (const char *)&packet, 3, 104); +#endif } #endif @@ -110,7 +136,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -120,7 +146,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -135,6 +161,6 @@ static inline void mavlink_msg_ctrl_srfc_pt_decode(const mavlink_message_t* msg, ctrl_srfc_pt->bitfieldPt = mavlink_msg_ctrl_srfc_pt_get_bitfieldPt(msg); ctrl_srfc_pt->target = mavlink_msg_ctrl_srfc_pt_get_target(msg); #else - memcpy(ctrl_srfc_pt, MAVLINK_PAYLOAD(msg), 3); + memcpy(ctrl_srfc_pt, _MAV_PAYLOAD(msg), 3); #endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index cf526b262..1a902e307 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - - put_float_by_index(msg, 0, fl_1); // Log value 1 - put_float_by_index(msg, 4, fl_2); // Log value 2 - put_float_by_index(msg, 8, fl_3); // Log value 3 - put_float_by_index(msg, 12, fl_4); // Log value 4 - put_float_by_index(msg, 16, fl_5); // Log value 5 - put_float_by_index(msg, 20, fl_6); // Log value 6 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_float(buf, 0, fl_1); + _mav_put_float(buf, 4, fl_2); + _mav_put_float(buf, 8, fl_3); + _mav_put_float(buf, 12, fl_4); + _mav_put_float(buf, 16, fl_5); + _mav_put_float(buf, 20, fl_6); + + memcpy(_MAV_PAYLOAD(msg), buf, 24); +#else + mavlink_data_log_t packet; + packet.fl_1 = fl_1; + packet.fl_2 = fl_2; + packet.fl_3 = fl_3; + packet.fl_4 = fl_4; + packet.fl_5 = fl_5; + packet.fl_6 = fl_6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_DATA_LOG; return mavlink_finalize_message(msg, system_id, component_id, 24, 167); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t mavlink_message_t* msg, float fl_1,float fl_2,float fl_3,float fl_4,float fl_5,float fl_6) { - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - - put_float_by_index(msg, 0, fl_1); // Log value 1 - put_float_by_index(msg, 4, fl_2); // Log value 2 - put_float_by_index(msg, 8, fl_3); // Log value 3 - put_float_by_index(msg, 12, fl_4); // Log value 4 - put_float_by_index(msg, 16, fl_5); // Log value 5 - put_float_by_index(msg, 20, fl_6); // Log value 6 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_float(buf, 0, fl_1); + _mav_put_float(buf, 4, fl_2); + _mav_put_float(buf, 8, fl_3); + _mav_put_float(buf, 12, fl_4); + _mav_put_float(buf, 16, fl_5); + _mav_put_float(buf, 20, fl_6); + + memcpy(_MAV_PAYLOAD(msg), buf, 24); +#else + mavlink_data_log_t packet; + packet.fl_1 = fl_1; + packet.fl_2 = fl_2; + packet.fl_3 = fl_3; + packet.fl_4 = fl_4; + packet.fl_5 = fl_5; + packet.fl_6 = fl_6; + + memcpy(_MAV_PAYLOAD(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_DATA_LOG; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 167); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co 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_ALIGNED_MESSAGE(msg, 24); - msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - - put_float_by_index(msg, 0, fl_1); // Log value 1 - put_float_by_index(msg, 4, fl_2); // Log value 2 - put_float_by_index(msg, 8, fl_3); // Log value 3 - put_float_by_index(msg, 12, fl_4); // Log value 4 - put_float_by_index(msg, 16, fl_5); // Log value 5 - put_float_by_index(msg, 20, fl_6); // Log value 6 - - mavlink_finalize_message_chan_send(msg, chan, 24, 167); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_float(buf, 0, fl_1); + _mav_put_float(buf, 4, fl_2); + _mav_put_float(buf, 8, fl_3); + _mav_put_float(buf, 12, fl_4); + _mav_put_float(buf, 16, fl_5); + _mav_put_float(buf, 20, fl_6); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_LOG, buf, 24, 167); +#else + mavlink_data_log_t packet; + packet.fl_1 = fl_1; + packet.fl_2 = fl_2; + packet.fl_3 = fl_3; + packet.fl_4 = fl_4; + packet.fl_5 = fl_5; + packet.fl_6 = fl_6; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DATA_LOG, (const char *)&packet, 24, 167); +#endif } #endif @@ -142,7 +180,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -152,7 +190,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -162,7 +200,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -172,7 +210,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -182,7 +220,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -192,7 +230,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_data_log_decode(const mavlink_message_t* msg, mav data_log->fl_5 = mavlink_msg_data_log_get_fl_5(msg); data_log->fl_6 = mavlink_msg_data_log_get_fl_6(msg); #else - memcpy(data_log, MAVLINK_PAYLOAD(msg), 24); + memcpy(data_log, _MAV_PAYLOAD(msg), 24); #endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index 05002adec..37449af86 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - - put_float_by_index(msg, 0, diagFl1); // Diagnostic float 1 - put_float_by_index(msg, 4, diagFl2); // Diagnostic float 2 - put_float_by_index(msg, 8, diagFl3); // Diagnostic float 3 - put_int16_t_by_index(msg, 12, diagSh1); // Diagnostic short 1 - put_int16_t_by_index(msg, 14, diagSh2); // Diagnostic short 2 - put_int16_t_by_index(msg, 16, diagSh3); // Diagnostic short 3 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, diagFl1); + _mav_put_float(buf, 4, diagFl2); + _mav_put_float(buf, 8, diagFl3); + _mav_put_int16_t(buf, 12, diagSh1); + _mav_put_int16_t(buf, 14, diagSh2); + _mav_put_int16_t(buf, 16, diagSh3); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_diagnostic_t packet; + packet.diagFl1 = diagFl1; + packet.diagFl2 = diagFl2; + packet.diagFl3 = diagFl3; + packet.diagSh1 = diagSh1; + packet.diagSh2 = diagSh2; + packet.diagSh3 = diagSh3; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; return mavlink_finalize_message(msg, system_id, component_id, 18, 2); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8 mavlink_message_t* msg, float diagFl1,float diagFl2,float diagFl3,int16_t diagSh1,int16_t diagSh2,int16_t diagSh3) { - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - - put_float_by_index(msg, 0, diagFl1); // Diagnostic float 1 - put_float_by_index(msg, 4, diagFl2); // Diagnostic float 2 - put_float_by_index(msg, 8, diagFl3); // Diagnostic float 3 - put_int16_t_by_index(msg, 12, diagSh1); // Diagnostic short 1 - put_int16_t_by_index(msg, 14, diagSh2); // Diagnostic short 2 - put_int16_t_by_index(msg, 16, diagSh3); // Diagnostic short 3 +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, diagFl1); + _mav_put_float(buf, 4, diagFl2); + _mav_put_float(buf, 8, diagFl3); + _mav_put_int16_t(buf, 12, diagSh1); + _mav_put_int16_t(buf, 14, diagSh2); + _mav_put_int16_t(buf, 16, diagSh3); + + memcpy(_MAV_PAYLOAD(msg), buf, 18); +#else + mavlink_diagnostic_t packet; + packet.diagFl1 = diagFl1; + packet.diagFl2 = diagFl2; + packet.diagFl3 = diagFl3; + packet.diagSh1 = diagSh1; + packet.diagSh2 = diagSh2; + packet.diagSh3 = diagSh3; + + memcpy(_MAV_PAYLOAD(msg), &packet, 18); +#endif + msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 2); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t 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_ALIGNED_MESSAGE(msg, 18); - msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - - put_float_by_index(msg, 0, diagFl1); // Diagnostic float 1 - put_float_by_index(msg, 4, diagFl2); // Diagnostic float 2 - put_float_by_index(msg, 8, diagFl3); // Diagnostic float 3 - put_int16_t_by_index(msg, 12, diagSh1); // Diagnostic short 1 - put_int16_t_by_index(msg, 14, diagSh2); // Diagnostic short 2 - put_int16_t_by_index(msg, 16, diagSh3); // Diagnostic short 3 - - mavlink_finalize_message_chan_send(msg, chan, 18, 2); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[18]; + _mav_put_float(buf, 0, diagFl1); + _mav_put_float(buf, 4, diagFl2); + _mav_put_float(buf, 8, diagFl3); + _mav_put_int16_t(buf, 12, diagSh1); + _mav_put_int16_t(buf, 14, diagSh2); + _mav_put_int16_t(buf, 16, diagSh3); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIAGNOSTIC, buf, 18, 2); +#else + mavlink_diagnostic_t packet; + packet.diagFl1 = diagFl1; + packet.diagFl2 = diagFl2; + packet.diagFl3 = diagFl3; + packet.diagSh1 = diagSh1; + packet.diagSh2 = diagSh2; + packet.diagSh3 = diagSh3; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DIAGNOSTIC, (const char *)&packet, 18, 2); +#endif } #endif @@ -142,7 +180,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -152,7 +190,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -162,7 +200,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -172,7 +210,7 @@ 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) { - return MAVLINK_MSG_RETURN_int16_t(msg, 12); + return _MAV_RETURN_int16_t(msg, 12); } /** @@ -182,7 +220,7 @@ 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) { - return MAVLINK_MSG_RETURN_int16_t(msg, 14); + return _MAV_RETURN_int16_t(msg, 14); } /** @@ -192,7 +230,7 @@ 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) { - return MAVLINK_MSG_RETURN_int16_t(msg, 16); + return _MAV_RETURN_int16_t(msg, 16); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_diagnostic_decode(const mavlink_message_t* msg, m diagnostic->diagSh2 = mavlink_msg_diagnostic_get_diagSh2(msg); diagnostic->diagSh3 = mavlink_msg_diagnostic_get_diagSh3(msg); #else - memcpy(diagnostic, MAVLINK_PAYLOAD(msg), 18); + memcpy(diagnostic, _MAV_PAYLOAD(msg), 18); #endif } 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 7ef93956f..e80ae1a28 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[7]; + _mav_put_uint8_t(buf, 0, year); + _mav_put_uint8_t(buf, 1, month); + _mav_put_uint8_t(buf, 2, day); + _mav_put_uint8_t(buf, 3, hour); + _mav_put_uint8_t(buf, 4, min); + _mav_put_uint8_t(buf, 5, sec); + _mav_put_uint8_t(buf, 6, visSat); + + memcpy(_MAV_PAYLOAD(msg), buf, 7); +#else + mavlink_gps_date_time_t packet; + packet.year = year; + packet.month = month; + packet.day = day; + packet.hour = hour; + packet.min = min; + packet.sec = sec; + packet.visSat = visSat; - put_uint8_t_by_index(msg, 0, year); // Year reported by Gps - put_uint8_t_by_index(msg, 1, month); // Month reported by Gps - put_uint8_t_by_index(msg, 2, day); // Day reported by Gps - put_uint8_t_by_index(msg, 3, hour); // Hour reported by Gps - put_uint8_t_by_index(msg, 4, min); // Min reported by Gps - put_uint8_t_by_index(msg, 5, sec); // Sec reported by Gps - put_uint8_t_by_index(msg, 6, visSat); // Visible sattelites reported by Gps + memcpy(_MAV_PAYLOAD(msg), &packet, 7); +#endif + msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; return mavlink_finalize_message(msg, system_id, component_id, 7, 16); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, ui 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) { - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[7]; + _mav_put_uint8_t(buf, 0, year); + _mav_put_uint8_t(buf, 1, month); + _mav_put_uint8_t(buf, 2, day); + _mav_put_uint8_t(buf, 3, hour); + _mav_put_uint8_t(buf, 4, min); + _mav_put_uint8_t(buf, 5, sec); + _mav_put_uint8_t(buf, 6, visSat); - put_uint8_t_by_index(msg, 0, year); // Year reported by Gps - put_uint8_t_by_index(msg, 1, month); // Month reported by Gps - put_uint8_t_by_index(msg, 2, day); // Day reported by Gps - put_uint8_t_by_index(msg, 3, hour); // Hour reported by Gps - put_uint8_t_by_index(msg, 4, min); // Min reported by Gps - put_uint8_t_by_index(msg, 5, sec); // Sec reported by Gps - put_uint8_t_by_index(msg, 6, visSat); // Visible sattelites reported by Gps + memcpy(_MAV_PAYLOAD(msg), buf, 7); +#else + mavlink_gps_date_time_t packet; + packet.year = year; + packet.month = month; + packet.day = day; + packet.hour = hour; + packet.min = min; + packet.sec = sec; + packet.visSat = visSat; + memcpy(_MAV_PAYLOAD(msg), &packet, 7); +#endif + + msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 7, 16); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 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_ALIGNED_MESSAGE(msg, 7); - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[7]; + _mav_put_uint8_t(buf, 0, year); + _mav_put_uint8_t(buf, 1, month); + _mav_put_uint8_t(buf, 2, day); + _mav_put_uint8_t(buf, 3, hour); + _mav_put_uint8_t(buf, 4, min); + _mav_put_uint8_t(buf, 5, sec); + _mav_put_uint8_t(buf, 6, visSat); - put_uint8_t_by_index(msg, 0, year); // Year reported by Gps - put_uint8_t_by_index(msg, 1, month); // Month reported by Gps - put_uint8_t_by_index(msg, 2, day); // Day reported by Gps - put_uint8_t_by_index(msg, 3, hour); // Hour reported by Gps - put_uint8_t_by_index(msg, 4, min); // Min reported by Gps - put_uint8_t_by_index(msg, 5, sec); // Sec reported by Gps - put_uint8_t_by_index(msg, 6, visSat); // Visible sattelites reported by Gps + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_DATE_TIME, buf, 7, 16); +#else + mavlink_gps_date_time_t packet; + packet.year = year; + packet.month = month; + packet.day = day; + packet.hour = hour; + packet.min = min; + packet.sec = sec; + packet.visSat = visSat; - mavlink_finalize_message_chan_send(msg, chan, 7, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_DATE_TIME, (const char *)&packet, 7, 16); +#endif } #endif @@ -150,7 +191,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -160,7 +201,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -170,7 +211,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -180,7 +221,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -190,7 +231,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 4); + return _MAV_RETURN_uint8_t(msg, 4); } /** @@ -200,7 +241,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 5); + return _MAV_RETURN_uint8_t(msg, 5); } /** @@ -210,7 +251,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 6); + return _MAV_RETURN_uint8_t(msg, 6); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_gps_date_time_decode(const mavlink_message_t* 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); #else - memcpy(gps_date_time, MAVLINK_PAYLOAD(msg), 7); + memcpy(gps_date_time, _MAV_PAYLOAD(msg), 7); #endif } 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 32d94ff58..77957a20a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -41,13 +41,25 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[13]; + _mav_put_float(buf, 0, hCommand); + _mav_put_float(buf, 4, uCommand); + _mav_put_float(buf, 8, rCommand); + _mav_put_uint8_t(buf, 12, target); + + memcpy(_MAV_PAYLOAD(msg), buf, 13); +#else + mavlink_mid_lvl_cmds_t packet; + packet.hCommand = hCommand; + packet.uCommand = uCommand; + packet.rCommand = rCommand; + packet.target = target; - put_float_by_index(msg, 0, hCommand); // Commanded Airspeed - put_float_by_index(msg, 4, uCommand); // Log value 2 - put_float_by_index(msg, 8, rCommand); // Log value 3 - put_uint8_t_by_index(msg, 12, target); // The system setting the commands + memcpy(_MAV_PAYLOAD(msg), &packet, 13); +#endif + msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; return mavlink_finalize_message(msg, system_id, component_id, 13, 146); } @@ -67,13 +79,25 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint8_t target,float hCommand,float uCommand,float rCommand) { - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[13]; + _mav_put_float(buf, 0, hCommand); + _mav_put_float(buf, 4, uCommand); + _mav_put_float(buf, 8, rCommand); + _mav_put_uint8_t(buf, 12, target); + + memcpy(_MAV_PAYLOAD(msg), buf, 13); +#else + mavlink_mid_lvl_cmds_t packet; + packet.hCommand = hCommand; + packet.uCommand = uCommand; + packet.rCommand = rCommand; + packet.target = target; - put_float_by_index(msg, 0, hCommand); // Commanded Airspeed - put_float_by_index(msg, 4, uCommand); // Log value 2 - put_float_by_index(msg, 8, rCommand); // Log value 3 - put_uint8_t_by_index(msg, 12, target); // The system setting the commands + memcpy(_MAV_PAYLOAD(msg), &packet, 13); +#endif + msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 146); } @@ -103,15 +127,23 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) { - MAVLINK_ALIGNED_MESSAGE(msg, 13); - msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - - put_float_by_index(msg, 0, hCommand); // Commanded Airspeed - put_float_by_index(msg, 4, uCommand); // Log value 2 - put_float_by_index(msg, 8, rCommand); // Log value 3 - put_uint8_t_by_index(msg, 12, target); // The system setting the commands +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[13]; + _mav_put_float(buf, 0, hCommand); + _mav_put_float(buf, 4, uCommand); + _mav_put_float(buf, 8, rCommand); + _mav_put_uint8_t(buf, 12, target); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MID_LVL_CMDS, buf, 13, 146); +#else + mavlink_mid_lvl_cmds_t packet; + packet.hCommand = hCommand; + packet.uCommand = uCommand; + packet.rCommand = rCommand; + packet.target = target; - mavlink_finalize_message_chan_send(msg, chan, 13, 146); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MID_LVL_CMDS, (const char *)&packet, 13, 146); +#endif } #endif @@ -126,7 +158,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 12); + return _MAV_RETURN_uint8_t(msg, 12); } /** @@ -136,7 +168,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -146,7 +178,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -156,7 +188,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -173,6 +205,6 @@ static inline void mavlink_msg_mid_lvl_cmds_decode(const mavlink_message_t* msg, mid_lvl_cmds->rCommand = mavlink_msg_mid_lvl_cmds_get_rCommand(msg); mid_lvl_cmds->target = mavlink_msg_mid_lvl_cmds_get_target(msg); #else - memcpy(mid_lvl_cmds, MAVLINK_PAYLOAD(msg), 13); + memcpy(mid_lvl_cmds, _MAV_PAYLOAD(msg), 13); #endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index fd5a367ac..729f30b90 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -47,15 +47,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - - put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s) - put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s) - put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s) - put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s) - put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s) - put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_float(buf, 0, axBias); + _mav_put_float(buf, 4, ayBias); + _mav_put_float(buf, 8, azBias); + _mav_put_float(buf, 12, gxBias); + _mav_put_float(buf, 16, gyBias); + _mav_put_float(buf, 20, gzBias); + + memcpy(_MAV_PAYLOAD(msg), buf, 24); +#else + mavlink_sensor_bias_t packet; + packet.axBias = axBias; + packet.ayBias = ayBias; + packet.azBias = azBias; + packet.gxBias = gxBias; + packet.gyBias = gyBias; + packet.gzBias = gzBias; + + memcpy(_MAV_PAYLOAD(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; return mavlink_finalize_message(msg, system_id, component_id, 24, 168); } @@ -77,15 +91,29 @@ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint mavlink_message_t* msg, float axBias,float ayBias,float azBias,float gxBias,float gyBias,float gzBias) { - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - - put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s) - put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s) - put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s) - put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s) - put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s) - put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s) +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_float(buf, 0, axBias); + _mav_put_float(buf, 4, ayBias); + _mav_put_float(buf, 8, azBias); + _mav_put_float(buf, 12, gxBias); + _mav_put_float(buf, 16, gyBias); + _mav_put_float(buf, 20, gzBias); + + memcpy(_MAV_PAYLOAD(msg), buf, 24); +#else + mavlink_sensor_bias_t packet; + packet.axBias = axBias; + packet.ayBias = ayBias; + packet.azBias = azBias; + packet.gxBias = gxBias; + packet.gyBias = gyBias; + packet.gzBias = gzBias; + + memcpy(_MAV_PAYLOAD(msg), &packet, 24); +#endif + msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 168); } @@ -117,17 +145,27 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t 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_ALIGNED_MESSAGE(msg, 24); - msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - - put_float_by_index(msg, 0, axBias); // Accelerometer X bias (m/s) - put_float_by_index(msg, 4, ayBias); // Accelerometer Y bias (m/s) - put_float_by_index(msg, 8, azBias); // Accelerometer Z bias (m/s) - put_float_by_index(msg, 12, gxBias); // Gyro X bias (rad/s) - put_float_by_index(msg, 16, gyBias); // Gyro Y bias (rad/s) - put_float_by_index(msg, 20, gzBias); // Gyro Z bias (rad/s) - - mavlink_finalize_message_chan_send(msg, chan, 24, 168); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[24]; + _mav_put_float(buf, 0, axBias); + _mav_put_float(buf, 4, ayBias); + _mav_put_float(buf, 8, azBias); + _mav_put_float(buf, 12, gxBias); + _mav_put_float(buf, 16, gyBias); + _mav_put_float(buf, 20, gzBias); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_BIAS, buf, 24, 168); +#else + mavlink_sensor_bias_t packet; + packet.axBias = axBias; + packet.ayBias = ayBias; + packet.azBias = azBias; + packet.gxBias = gxBias; + packet.gyBias = gyBias; + packet.gzBias = gzBias; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SENSOR_BIAS, (const char *)&packet, 24, 168); +#endif } #endif @@ -142,7 +180,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -152,7 +190,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -162,7 +200,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -172,7 +210,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -182,7 +220,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -192,7 +230,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -211,6 +249,6 @@ static inline void mavlink_msg_sensor_bias_decode(const mavlink_message_t* msg, sensor_bias->gyBias = mavlink_msg_sensor_bias_get_gyBias(msg); sensor_bias->gzBias = mavlink_msg_sensor_bias_get_gzBias(msg); #else - memcpy(sensor_bias, MAVLINK_PAYLOAD(msg), 24); + memcpy(sensor_bias, _MAV_PAYLOAD(msg), 24); #endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index 765d6332d..71691304f 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, actionVal); + _mav_put_uint8_t(buf, 2, target); + _mav_put_uint8_t(buf, 3, actionId); + + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_slugs_action_t packet; + packet.actionVal = actionVal; + packet.target = target; + packet.actionId = actionId; - put_uint16_t_by_index(msg, 0, actionVal); // Value associated with the action - put_uint8_t_by_index(msg, 2, target); // The system reporting the action - put_uint8_t_by_index(msg, 3, actionId); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; return mavlink_finalize_message(msg, system_id, component_id, 4, 65); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uin mavlink_message_t* msg, uint8_t target,uint8_t actionId,uint16_t actionVal) { - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, actionVal); + _mav_put_uint8_t(buf, 2, target); + _mav_put_uint8_t(buf, 3, actionId); - put_uint16_t_by_index(msg, 0, actionVal); // Value associated with the action - put_uint8_t_by_index(msg, 2, target); // The system reporting the action - put_uint8_t_by_index(msg, 3, actionId); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + memcpy(_MAV_PAYLOAD(msg), buf, 4); +#else + mavlink_slugs_action_t packet; + packet.actionVal = actionVal; + packet.target = target; + packet.actionId = actionId; + memcpy(_MAV_PAYLOAD(msg), &packet, 4); +#endif + + msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 65); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) { - MAVLINK_ALIGNED_MESSAGE(msg, 4); - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[4]; + _mav_put_uint16_t(buf, 0, actionVal); + _mav_put_uint8_t(buf, 2, target); + _mav_put_uint8_t(buf, 3, actionId); - put_uint16_t_by_index(msg, 0, actionVal); // Value associated with the action - put_uint8_t_by_index(msg, 2, target); // The system reporting the action - put_uint8_t_by_index(msg, 3, actionId); // Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_ACTION, buf, 4, 65); +#else + mavlink_slugs_action_t packet; + packet.actionVal = actionVal; + packet.target = target; + packet.actionId = actionId; - mavlink_finalize_message_chan_send(msg, chan, 4, 65); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_ACTION, (const char *)&packet, 4, 65); +#endif } #endif @@ -118,7 +147,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -128,7 +157,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 3); + return _MAV_RETURN_uint8_t(msg, 3); } /** @@ -138,7 +167,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 0); + return _MAV_RETURN_uint16_t(msg, 0); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_slugs_action_decode(const mavlink_message_t* msg, slugs_action->target = mavlink_msg_slugs_action_get_target(msg); slugs_action->actionId = mavlink_msg_slugs_action_get_actionId(msg); #else - memcpy(slugs_action, MAVLINK_PAYLOAD(msg), 4); + memcpy(slugs_action, _MAV_PAYLOAD(msg), 4); #endif } diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index 2e20f14a4..4df72dcce 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -56,18 +56,35 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_float(buf, 0, u_m); + _mav_put_float(buf, 4, phi_c); + _mav_put_float(buf, 8, theta_c); + _mav_put_float(buf, 12, psiDot_c); + _mav_put_float(buf, 16, ay_body); + _mav_put_float(buf, 20, totalDist); + _mav_put_float(buf, 24, dist2Go); + _mav_put_uint8_t(buf, 28, fromWP); + _mav_put_uint8_t(buf, 29, toWP); + + memcpy(_MAV_PAYLOAD(msg), buf, 30); +#else + mavlink_slugs_navigation_t packet; + packet.u_m = u_m; + packet.phi_c = phi_c; + packet.theta_c = theta_c; + packet.psiDot_c = psiDot_c; + packet.ay_body = ay_body; + packet.totalDist = totalDist; + packet.dist2Go = dist2Go; + packet.fromWP = fromWP; + packet.toWP = toWP; - put_float_by_index(msg, 0, u_m); // Measured Airspeed prior to the Nav Filter - put_float_by_index(msg, 4, phi_c); // Commanded Roll - put_float_by_index(msg, 8, theta_c); // Commanded Pitch - put_float_by_index(msg, 12, psiDot_c); // Commanded Turn rate - put_float_by_index(msg, 16, ay_body); // Y component of the body acceleration - put_float_by_index(msg, 20, totalDist); // Total Distance to Run on this leg of Navigation - put_float_by_index(msg, 24, dist2Go); // Remaining distance to Run on this leg of Navigation - put_uint8_t_by_index(msg, 28, fromWP); // Origin WP - put_uint8_t_by_index(msg, 29, toWP); // Destination WP + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; return mavlink_finalize_message(msg, system_id, component_id, 30, 120); } @@ -92,18 +109,35 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_float(buf, 0, u_m); + _mav_put_float(buf, 4, phi_c); + _mav_put_float(buf, 8, theta_c); + _mav_put_float(buf, 12, psiDot_c); + _mav_put_float(buf, 16, ay_body); + _mav_put_float(buf, 20, totalDist); + _mav_put_float(buf, 24, dist2Go); + _mav_put_uint8_t(buf, 28, fromWP); + _mav_put_uint8_t(buf, 29, toWP); + + memcpy(_MAV_PAYLOAD(msg), buf, 30); +#else + mavlink_slugs_navigation_t packet; + packet.u_m = u_m; + packet.phi_c = phi_c; + packet.theta_c = theta_c; + packet.psiDot_c = psiDot_c; + packet.ay_body = ay_body; + packet.totalDist = totalDist; + packet.dist2Go = dist2Go; + packet.fromWP = fromWP; + packet.toWP = toWP; - put_float_by_index(msg, 0, u_m); // Measured Airspeed prior to the Nav Filter - put_float_by_index(msg, 4, phi_c); // Commanded Roll - put_float_by_index(msg, 8, theta_c); // Commanded Pitch - put_float_by_index(msg, 12, psiDot_c); // Commanded Turn rate - put_float_by_index(msg, 16, ay_body); // Y component of the body acceleration - put_float_by_index(msg, 20, totalDist); // Total Distance to Run on this leg of Navigation - put_float_by_index(msg, 24, dist2Go); // Remaining distance to Run on this leg of Navigation - put_uint8_t_by_index(msg, 28, fromWP); // Origin WP - put_uint8_t_by_index(msg, 29, toWP); // Destination WP + memcpy(_MAV_PAYLOAD(msg), &packet, 30); +#endif + msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 120); } @@ -138,20 +172,33 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui 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_ALIGNED_MESSAGE(msg, 30); - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[30]; + _mav_put_float(buf, 0, u_m); + _mav_put_float(buf, 4, phi_c); + _mav_put_float(buf, 8, theta_c); + _mav_put_float(buf, 12, psiDot_c); + _mav_put_float(buf, 16, ay_body); + _mav_put_float(buf, 20, totalDist); + _mav_put_float(buf, 24, dist2Go); + _mav_put_uint8_t(buf, 28, fromWP); + _mav_put_uint8_t(buf, 29, toWP); - put_float_by_index(msg, 0, u_m); // Measured Airspeed prior to the Nav Filter - put_float_by_index(msg, 4, phi_c); // Commanded Roll - put_float_by_index(msg, 8, theta_c); // Commanded Pitch - put_float_by_index(msg, 12, psiDot_c); // Commanded Turn rate - put_float_by_index(msg, 16, ay_body); // Y component of the body acceleration - put_float_by_index(msg, 20, totalDist); // Total Distance to Run on this leg of Navigation - put_float_by_index(msg, 24, dist2Go); // Remaining distance to Run on this leg of Navigation - put_uint8_t_by_index(msg, 28, fromWP); // Origin WP - put_uint8_t_by_index(msg, 29, toWP); // Destination WP - - mavlink_finalize_message_chan_send(msg, chan, 30, 120); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION, buf, 30, 120); +#else + mavlink_slugs_navigation_t packet; + packet.u_m = u_m; + packet.phi_c = phi_c; + packet.theta_c = theta_c; + packet.psiDot_c = psiDot_c; + packet.ay_body = ay_body; + packet.totalDist = totalDist; + packet.dist2Go = dist2Go; + packet.fromWP = fromWP; + packet.toWP = toWP; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION, (const char *)&packet, 30, 120); +#endif } #endif @@ -166,7 +213,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 0); + return _MAV_RETURN_float(msg, 0); } /** @@ -176,7 +223,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 4); + return _MAV_RETURN_float(msg, 4); } /** @@ -186,7 +233,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -196,7 +243,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -206,7 +253,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -216,7 +263,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -226,7 +273,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -236,7 +283,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 28); + return _MAV_RETURN_uint8_t(msg, 28); } /** @@ -246,7 +293,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 29); + return _MAV_RETURN_uint8_t(msg, 29); } /** @@ -268,6 +315,6 @@ static inline void mavlink_msg_slugs_navigation_decode(const mavlink_message_t* slugs_navigation->fromWP = mavlink_msg_slugs_navigation_get_fromWP(msg); slugs_navigation->toWP = mavlink_msg_slugs_navigation_get_toWP(msg); #else - memcpy(slugs_navigation, MAVLINK_PAYLOAD(msg), 30); + memcpy(slugs_navigation, _MAV_PAYLOAD(msg), 30); #endif } diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index ec09108d5..4726f931f 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -20,7 +20,11 @@ extern "C" { #endif #ifndef MAVLINK_MESSAGE_INFO +<<<<<<< HEAD #define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_CPU_LOAD, MAVLINK_MESSAGE_INFO_AIR_DATA, MAVLINK_MESSAGE_INFO_SENSOR_BIAS, MAVLINK_MESSAGE_INFO_DIAGNOSTIC, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SLUGS_NAVIGATION, MAVLINK_MESSAGE_INFO_DATA_LOG, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_GPS_DATE_TIME, MAVLINK_MESSAGE_INFO_MID_LVL_CMDS, MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_SLUGS_ACTION, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +======= +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {NULL}, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {NULL}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {NULL}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_CPU_LOAD, MAVLINK_MESSAGE_INFO_AIR_DATA, MAVLINK_MESSAGE_INFO_SENSOR_BIAS, MAVLINK_MESSAGE_INFO_DIAGNOSTIC, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SLUGS_NAVIGATION, MAVLINK_MESSAGE_INFO_DATA_LOG, {NULL}, MAVLINK_MESSAGE_INFO_GPS_DATE_TIME, MAVLINK_MESSAGE_INFO_MID_LVL_CMDS, MAVLINK_MESSAGE_INFO_CTRL_SRFC_PT, {NULL}, MAVLINK_MESSAGE_INFO_SLUGS_ACTION, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +>>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/slugs/testsuite.h b/thirdParty/mavlink/include/slugs/testsuite.h index ba8162cd1..4593235a4 100644 --- a/thirdParty/mavlink/include/slugs/testsuite.h +++ b/thirdParty/mavlink/include/slugs/testsuite.h @@ -11,75 +11,119 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t); -static void mavlink_test_slugs(uint8_t, uint8_t); +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_slugs(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_common(system_id, component_id); - mavlink_test_slugs(system_id, component_id); + mavlink_test_common(system_id, component_id, last_msg); + mavlink_test_slugs(system_id, component_id, last_msg); } #endif #include "../common/testsuite.h" -static void mavlink_test_cpu_load(uint8_t system_id, uint8_t component_id) +static void mavlink_test_cpu_load(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_cpu_load_t packet2, packet1 = { + mavlink_cpu_load_t packet_in = { 17235, 139, 206, }; - if (sizeof(packet2) != 4) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_cpu_load_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.batVolt = packet_in.batVolt; + packet1.sensLoad = packet_in.sensLoad; + packet1.ctrlLoad = packet_in.ctrlLoad; + + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_cpu_load_encode(system_id, component_id, &msg, &packet1); mavlink_msg_cpu_load_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_cpu_load_pack(system_id, component_id, &msg , packet1.sensLoad , packet1.ctrlLoad , packet1.batVolt ); + mavlink_msg_cpu_load_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_cpu_load_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.sensLoad , packet1.ctrlLoad , packet1.batVolt ); + mavlink_msg_cpu_load_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #include "mavlink.h" diff --git a/thirdParty/mavlink/include/test/mavlink_msg_test_types.h b/thirdParty/mavlink/include/test/mavlink_msg_test_types.h index 6b5f6723f..0850d0e6b 100644 --- a/thirdParty/mavlink/include/test/mavlink_msg_test_types.h +++ b/thirdParty/mavlink/include/test/mavlink_msg_test_types.h @@ -31,17 +31,17 @@ typedef struct __mavlink_test_types_t #define MAVLINK_MSG_ID_TEST_TYPES_LEN 179 #define MAVLINK_MSG_ID_0_LEN 179 -#define MAVLINK_MSG_TEST_TYPES_FIELD_S_LEN 10 -#define MAVLINK_MSG_TEST_TYPES_FIELD_U8_ARRAY_LEN 3 -#define MAVLINK_MSG_TEST_TYPES_FIELD_U16_ARRAY_LEN 3 -#define MAVLINK_MSG_TEST_TYPES_FIELD_U32_ARRAY_LEN 3 #define MAVLINK_MSG_TEST_TYPES_FIELD_U64_ARRAY_LEN 3 -#define MAVLINK_MSG_TEST_TYPES_FIELD_S8_ARRAY_LEN 3 -#define MAVLINK_MSG_TEST_TYPES_FIELD_S16_ARRAY_LEN 3 -#define MAVLINK_MSG_TEST_TYPES_FIELD_S32_ARRAY_LEN 3 #define MAVLINK_MSG_TEST_TYPES_FIELD_S64_ARRAY_LEN 3 -#define MAVLINK_MSG_TEST_TYPES_FIELD_F_ARRAY_LEN 3 #define MAVLINK_MSG_TEST_TYPES_FIELD_D_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_U32_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_S32_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_F_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_U16_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_S16_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_S_LEN 10 +#define MAVLINK_MSG_TEST_TYPES_FIELD_U8_ARRAY_LEN 3 +#define MAVLINK_MSG_TEST_TYPES_FIELD_S8_ARRAY_LEN 3 #define MAVLINK_MESSAGE_INFO_TEST_TYPES { \ "TEST_TYPES", \ @@ -105,31 +105,59 @@ typedef struct __mavlink_test_types_t static inline uint16_t mavlink_msg_test_types_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array) { - msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; - - put_uint64_t_by_index(msg, 0, u64); // uint64_t - put_int64_t_by_index(msg, 8, s64); // int64_t - put_double_by_index(msg, 16, d); // double - put_uint64_t_array_by_index(msg, 24, u64_array, 3); // uint64_t_array - put_int64_t_array_by_index(msg, 48, s64_array, 3); // int64_t_array - put_double_array_by_index(msg, 72, d_array, 3); // double_array - put_uint32_t_by_index(msg, 96, u32); // uint32_t - put_int32_t_by_index(msg, 100, s32); // int32_t - put_float_by_index(msg, 104, f); // float - put_uint32_t_array_by_index(msg, 108, u32_array, 3); // uint32_t_array - put_int32_t_array_by_index(msg, 120, s32_array, 3); // int32_t_array - put_float_array_by_index(msg, 132, f_array, 3); // float_array - put_uint16_t_by_index(msg, 144, u16); // uint16_t - put_int16_t_by_index(msg, 146, s16); // int16_t - put_uint16_t_array_by_index(msg, 148, u16_array, 3); // uint16_t_array - put_int16_t_array_by_index(msg, 154, s16_array, 3); // int16_t_array - put_char_by_index(msg, 160, c); // char - put_char_array_by_index(msg, 161, s, 10); // string - put_uint8_t_by_index(msg, 171, u8); // uint8_t - put_int8_t_by_index(msg, 172, s8); // int8_t - put_uint8_t_array_by_index(msg, 173, u8_array, 3); // uint8_t_array - put_int8_t_array_by_index(msg, 176, s8_array, 3); // int8_t_array +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[179]; + _mav_put_uint64_t(buf, 0, u64); + _mav_put_int64_t(buf, 8, s64); + _mav_put_double(buf, 16, d); + _mav_put_uint32_t(buf, 96, u32); + _mav_put_int32_t(buf, 100, s32); + _mav_put_float(buf, 104, f); + _mav_put_uint16_t(buf, 144, u16); + _mav_put_int16_t(buf, 146, s16); + _mav_put_char(buf, 160, c); + _mav_put_uint8_t(buf, 171, u8); + _mav_put_int8_t(buf, 172, s8); + _mav_put_uint64_t_array(buf, 24, u64_array, 3); + _mav_put_int64_t_array(buf, 48, s64_array, 3); + _mav_put_double_array(buf, 72, d_array, 3); + _mav_put_uint32_t_array(buf, 108, u32_array, 3); + _mav_put_int32_t_array(buf, 120, s32_array, 3); + _mav_put_float_array(buf, 132, f_array, 3); + _mav_put_uint16_t_array(buf, 148, u16_array, 3); + _mav_put_int16_t_array(buf, 154, s16_array, 3); + _mav_put_char_array(buf, 161, s, 10); + _mav_put_uint8_t_array(buf, 173, u8_array, 3); + _mav_put_int8_t_array(buf, 176, s8_array, 3); + memcpy(_MAV_PAYLOAD(msg), buf, 179); +#else + mavlink_test_types_t packet; + packet.u64 = u64; + packet.s64 = s64; + packet.d = d; + packet.u32 = u32; + packet.s32 = s32; + packet.f = f; + packet.u16 = u16; + packet.s16 = s16; + packet.c = c; + packet.u8 = u8; + packet.s8 = s8; + memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); + memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); + memcpy(packet.d_array, d_array, sizeof(double)*3); + memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); + memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); + memcpy(packet.f_array, f_array, sizeof(float)*3); + memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); + memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); + memcpy(packet.s, s, sizeof(char)*10); + memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); + memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); + memcpy(_MAV_PAYLOAD(msg), &packet, 179); +#endif + msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; return mavlink_finalize_message(msg, system_id, component_id, 179, 103); } @@ -167,31 +195,59 @@ static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8 mavlink_message_t* msg, char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array) { - msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; - - put_uint64_t_by_index(msg, 0, u64); // uint64_t - put_int64_t_by_index(msg, 8, s64); // int64_t - put_double_by_index(msg, 16, d); // double - put_uint64_t_array_by_index(msg, 24, u64_array, 3); // uint64_t_array - put_int64_t_array_by_index(msg, 48, s64_array, 3); // int64_t_array - put_double_array_by_index(msg, 72, d_array, 3); // double_array - put_uint32_t_by_index(msg, 96, u32); // uint32_t - put_int32_t_by_index(msg, 100, s32); // int32_t - put_float_by_index(msg, 104, f); // float - put_uint32_t_array_by_index(msg, 108, u32_array, 3); // uint32_t_array - put_int32_t_array_by_index(msg, 120, s32_array, 3); // int32_t_array - put_float_array_by_index(msg, 132, f_array, 3); // float_array - put_uint16_t_by_index(msg, 144, u16); // uint16_t - put_int16_t_by_index(msg, 146, s16); // int16_t - put_uint16_t_array_by_index(msg, 148, u16_array, 3); // uint16_t_array - put_int16_t_array_by_index(msg, 154, s16_array, 3); // int16_t_array - put_char_by_index(msg, 160, c); // char - put_char_array_by_index(msg, 161, s, 10); // string - put_uint8_t_by_index(msg, 171, u8); // uint8_t - put_int8_t_by_index(msg, 172, s8); // int8_t - put_uint8_t_array_by_index(msg, 173, u8_array, 3); // uint8_t_array - put_int8_t_array_by_index(msg, 176, s8_array, 3); // int8_t_array +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[179]; + _mav_put_uint64_t(buf, 0, u64); + _mav_put_int64_t(buf, 8, s64); + _mav_put_double(buf, 16, d); + _mav_put_uint32_t(buf, 96, u32); + _mav_put_int32_t(buf, 100, s32); + _mav_put_float(buf, 104, f); + _mav_put_uint16_t(buf, 144, u16); + _mav_put_int16_t(buf, 146, s16); + _mav_put_char(buf, 160, c); + _mav_put_uint8_t(buf, 171, u8); + _mav_put_int8_t(buf, 172, s8); + _mav_put_uint64_t_array(buf, 24, u64_array, 3); + _mav_put_int64_t_array(buf, 48, s64_array, 3); + _mav_put_double_array(buf, 72, d_array, 3); + _mav_put_uint32_t_array(buf, 108, u32_array, 3); + _mav_put_int32_t_array(buf, 120, s32_array, 3); + _mav_put_float_array(buf, 132, f_array, 3); + _mav_put_uint16_t_array(buf, 148, u16_array, 3); + _mav_put_int16_t_array(buf, 154, s16_array, 3); + _mav_put_char_array(buf, 161, s, 10); + _mav_put_uint8_t_array(buf, 173, u8_array, 3); + _mav_put_int8_t_array(buf, 176, s8_array, 3); + memcpy(_MAV_PAYLOAD(msg), buf, 179); +#else + mavlink_test_types_t packet; + packet.u64 = u64; + packet.s64 = s64; + packet.d = d; + packet.u32 = u32; + packet.s32 = s32; + packet.f = f; + packet.u16 = u16; + packet.s16 = s16; + packet.c = c; + packet.u8 = u8; + packet.s8 = s8; + memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); + memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); + memcpy(packet.d_array, d_array, sizeof(double)*3); + memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); + memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); + memcpy(packet.f_array, f_array, sizeof(float)*3); + memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); + memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); + memcpy(packet.s, s, sizeof(char)*10); + memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); + memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); + memcpy(_MAV_PAYLOAD(msg), &packet, 179); +#endif + msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179, 103); } @@ -239,33 +295,57 @@ static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array) { - MAVLINK_ALIGNED_MESSAGE(msg, 179); - msg->msgid = MAVLINK_MSG_ID_TEST_TYPES; - - put_uint64_t_by_index(msg, 0, u64); // uint64_t - put_int64_t_by_index(msg, 8, s64); // int64_t - put_double_by_index(msg, 16, d); // double - put_uint64_t_array_by_index(msg, 24, u64_array, 3); // uint64_t_array - put_int64_t_array_by_index(msg, 48, s64_array, 3); // int64_t_array - put_double_array_by_index(msg, 72, d_array, 3); // double_array - put_uint32_t_by_index(msg, 96, u32); // uint32_t - put_int32_t_by_index(msg, 100, s32); // int32_t - put_float_by_index(msg, 104, f); // float - put_uint32_t_array_by_index(msg, 108, u32_array, 3); // uint32_t_array - put_int32_t_array_by_index(msg, 120, s32_array, 3); // int32_t_array - put_float_array_by_index(msg, 132, f_array, 3); // float_array - put_uint16_t_by_index(msg, 144, u16); // uint16_t - put_int16_t_by_index(msg, 146, s16); // int16_t - put_uint16_t_array_by_index(msg, 148, u16_array, 3); // uint16_t_array - put_int16_t_array_by_index(msg, 154, s16_array, 3); // int16_t_array - put_char_by_index(msg, 160, c); // char - put_char_array_by_index(msg, 161, s, 10); // string - put_uint8_t_by_index(msg, 171, u8); // uint8_t - put_int8_t_by_index(msg, 172, s8); // int8_t - put_uint8_t_array_by_index(msg, 173, u8_array, 3); // uint8_t_array - put_int8_t_array_by_index(msg, 176, s8_array, 3); // int8_t_array - - mavlink_finalize_message_chan_send(msg, chan, 179, 103); +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[179]; + _mav_put_uint64_t(buf, 0, u64); + _mav_put_int64_t(buf, 8, s64); + _mav_put_double(buf, 16, d); + _mav_put_uint32_t(buf, 96, u32); + _mav_put_int32_t(buf, 100, s32); + _mav_put_float(buf, 104, f); + _mav_put_uint16_t(buf, 144, u16); + _mav_put_int16_t(buf, 146, s16); + _mav_put_char(buf, 160, c); + _mav_put_uint8_t(buf, 171, u8); + _mav_put_int8_t(buf, 172, s8); + _mav_put_uint64_t_array(buf, 24, u64_array, 3); + _mav_put_int64_t_array(buf, 48, s64_array, 3); + _mav_put_double_array(buf, 72, d_array, 3); + _mav_put_uint32_t_array(buf, 108, u32_array, 3); + _mav_put_int32_t_array(buf, 120, s32_array, 3); + _mav_put_float_array(buf, 132, f_array, 3); + _mav_put_uint16_t_array(buf, 148, u16_array, 3); + _mav_put_int16_t_array(buf, 154, s16_array, 3); + _mav_put_char_array(buf, 161, s, 10); + _mav_put_uint8_t_array(buf, 173, u8_array, 3); + _mav_put_int8_t_array(buf, 176, s8_array, 3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179, 103); +#else + mavlink_test_types_t packet; + packet.u64 = u64; + packet.s64 = s64; + packet.d = d; + packet.u32 = u32; + packet.s32 = s32; + packet.f = f; + packet.u16 = u16; + packet.s16 = s16; + packet.c = c; + packet.u8 = u8; + packet.s8 = s8; + memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3); + memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3); + memcpy(packet.d_array, d_array, sizeof(double)*3); + memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3); + memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3); + memcpy(packet.f_array, f_array, sizeof(float)*3); + memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3); + memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3); + memcpy(packet.s, s, sizeof(char)*10); + memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3); + memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179, 103); +#endif } #endif @@ -280,7 +360,7 @@ static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, c */ static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_char(msg, 160); + return _MAV_RETURN_char(msg, 160); } /** @@ -290,7 +370,7 @@ static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg) */ static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s) { - return MAVLINK_MSG_RETURN_char_array(msg, s, 10, 161); + return _MAV_RETURN_char_array(msg, s, 10, 161); } /** @@ -300,7 +380,7 @@ static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg */ static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint8_t(msg, 171); + return _MAV_RETURN_uint8_t(msg, 171); } /** @@ -310,7 +390,7 @@ static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg */ static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint16_t(msg, 144); + return _MAV_RETURN_uint16_t(msg, 144); } /** @@ -320,7 +400,7 @@ static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* m */ static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint32_t(msg, 96); + return _MAV_RETURN_uint32_t(msg, 96); } /** @@ -330,7 +410,7 @@ static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* m */ static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -340,7 +420,7 @@ static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* m */ static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int8_t(msg, 172); + return _MAV_RETURN_int8_t(msg, 172); } /** @@ -350,7 +430,7 @@ static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg) */ static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int16_t(msg, 146); + return _MAV_RETURN_int16_t(msg, 146); } /** @@ -360,7 +440,7 @@ static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* ms */ static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int32_t(msg, 100); + return _MAV_RETURN_int32_t(msg, 100); } /** @@ -370,7 +450,7 @@ static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* ms */ static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_int64_t(msg, 8); + return _MAV_RETURN_int64_t(msg, 8); } /** @@ -380,7 +460,7 @@ static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* ms */ static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_float(msg, 104); + return _MAV_RETURN_float(msg, 104); } /** @@ -390,7 +470,7 @@ static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg) */ static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg) { - return MAVLINK_MSG_RETURN_double(msg, 16); + return _MAV_RETURN_double(msg, 16); } /** @@ -400,7 +480,7 @@ static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg) */ static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array) { - return MAVLINK_MSG_RETURN_uint8_t_array(msg, u8_array, 3, 173); + return _MAV_RETURN_uint8_t_array(msg, u8_array, 3, 173); } /** @@ -410,7 +490,7 @@ static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message */ static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, u16_array, 3, 148); + return _MAV_RETURN_uint16_t_array(msg, u16_array, 3, 148); } /** @@ -420,7 +500,7 @@ static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_messag */ static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array) { - return MAVLINK_MSG_RETURN_uint32_t_array(msg, u32_array, 3, 108); + return _MAV_RETURN_uint32_t_array(msg, u32_array, 3, 108); } /** @@ -430,7 +510,7 @@ static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_messag */ static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array) { - return MAVLINK_MSG_RETURN_uint64_t_array(msg, u64_array, 3, 24); + return _MAV_RETURN_uint64_t_array(msg, u64_array, 3, 24); } /** @@ -440,7 +520,7 @@ static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_messag */ static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array) { - return MAVLINK_MSG_RETURN_int8_t_array(msg, s8_array, 3, 176); + return _MAV_RETURN_int8_t_array(msg, s8_array, 3, 176); } /** @@ -450,7 +530,7 @@ static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message */ static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array) { - return MAVLINK_MSG_RETURN_int16_t_array(msg, s16_array, 3, 154); + return _MAV_RETURN_int16_t_array(msg, s16_array, 3, 154); } /** @@ -460,7 +540,7 @@ static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_messag */ static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array) { - return MAVLINK_MSG_RETURN_int32_t_array(msg, s32_array, 3, 120); + return _MAV_RETURN_int32_t_array(msg, s32_array, 3, 120); } /** @@ -470,7 +550,7 @@ static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_messag */ static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array) { - return MAVLINK_MSG_RETURN_int64_t_array(msg, s64_array, 3, 48); + return _MAV_RETURN_int64_t_array(msg, s64_array, 3, 48); } /** @@ -480,7 +560,7 @@ static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_messag */ static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array) { - return MAVLINK_MSG_RETURN_float_array(msg, f_array, 3, 132); + return _MAV_RETURN_float_array(msg, f_array, 3, 132); } /** @@ -490,7 +570,7 @@ static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_ */ static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array) { - return MAVLINK_MSG_RETURN_double_array(msg, d_array, 3, 72); + return _MAV_RETURN_double_array(msg, d_array, 3, 72); } /** @@ -525,6 +605,6 @@ static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, m mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array); mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array); #else - memcpy(test_types, MAVLINK_PAYLOAD(msg), 179); + memcpy(test_types, _MAV_PAYLOAD(msg), 179); #endif } diff --git a/thirdParty/mavlink/include/test/test.h b/thirdParty/mavlink/include/test/test.h index 10b281917..ef00846f7 100644 --- a/thirdParty/mavlink/include/test/test.h +++ b/thirdParty/mavlink/include/test/test.h @@ -20,7 +20,11 @@ extern "C" { #endif #ifndef MAVLINK_MESSAGE_INFO +<<<<<<< HEAD #define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}, {"EMPTY",0,{}}} +======= +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}} +>>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/test/testsuite.h b/thirdParty/mavlink/include/test/testsuite.h index 4553fc730..bfb269fb5 100644 --- a/thirdParty/mavlink/include/test/testsuite.h +++ b/thirdParty/mavlink/include/test/testsuite.h @@ -12,24 +12,24 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_test(uint8_t, uint8_t); +static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_test(system_id, component_id); + mavlink_test_test(system_id, component_id, last_msg); } #endif -static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id) +static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_test_types_t packet2, packet1 = { + mavlink_test_types_t packet_in = { 93372036854775807ULL, 93372036854776311LL, 235.0, @@ -53,24 +53,65 @@ static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id) { 76, 77, 78 }, { 21, 22, 23 }, }; - if (sizeof(packet2) != 179) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_test_types_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.u64 = packet_in.u64; + packet1.s64 = packet_in.s64; + packet1.d = packet_in.d; + packet1.u32 = packet_in.u32; + packet1.s32 = packet_in.s32; + packet1.f = packet_in.f; + packet1.u16 = packet_in.u16; + packet1.s16 = packet_in.s16; + packet1.c = packet_in.c; + packet1.u8 = packet_in.u8; + packet1.s8 = packet_in.s8; + + memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3); + memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3); + memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3); + memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3); + memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3); + memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3); + memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3); + memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3); + memcpy(packet1.s, packet_in.s, sizeof(char)*10); + memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3); + memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3); + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1); mavlink_msg_test_types_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); + mavlink_msg_test_types_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array ); + mavlink_msg_test_types_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #include "mavlink.h" 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 52676a31c..25a654684 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -50,16 +50,31 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, accel_0); + _mav_put_float(buf, 12, accel_1); + _mav_put_float(buf, 16, accel_2); + _mav_put_float(buf, 20, gyro_0); + _mav_put_float(buf, 24, gyro_1); + _mav_put_float(buf, 28, gyro_2); + + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_nav_filter_bias_t packet; + packet.usec = usec; + packet.accel_0 = accel_0; + packet.accel_1 = accel_1; + packet.accel_2 = accel_2; + packet.gyro_0 = gyro_0; + packet.gyro_1 = gyro_1; + packet.gyro_2 = gyro_2; - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds) - put_float_by_index(msg, 8, accel_0); // b_f[0] - put_float_by_index(msg, 12, accel_1); // b_f[1] - put_float_by_index(msg, 16, accel_2); // b_f[2] - put_float_by_index(msg, 20, gyro_0); // b_f[0] - put_float_by_index(msg, 24, gyro_1); // b_f[1] - put_float_by_index(msg, 28, gyro_2); // b_f[2] + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; return mavlink_finalize_message(msg, system_id, component_id, 32, 34); } @@ -82,16 +97,31 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, accel_0); + _mav_put_float(buf, 12, accel_1); + _mav_put_float(buf, 16, accel_2); + _mav_put_float(buf, 20, gyro_0); + _mav_put_float(buf, 24, gyro_1); + _mav_put_float(buf, 28, gyro_2); - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds) - put_float_by_index(msg, 8, accel_0); // b_f[0] - put_float_by_index(msg, 12, accel_1); // b_f[1] - put_float_by_index(msg, 16, accel_2); // b_f[2] - put_float_by_index(msg, 20, gyro_0); // b_f[0] - put_float_by_index(msg, 24, gyro_1); // b_f[1] - put_float_by_index(msg, 28, gyro_2); // b_f[2] + memcpy(_MAV_PAYLOAD(msg), buf, 32); +#else + mavlink_nav_filter_bias_t packet; + packet.usec = usec; + packet.accel_0 = accel_0; + packet.accel_1 = accel_1; + packet.accel_2 = accel_2; + packet.gyro_0 = gyro_0; + packet.gyro_1 = gyro_1; + packet.gyro_2 = gyro_2; + memcpy(_MAV_PAYLOAD(msg), &packet, 32); +#endif + + msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 34); } @@ -124,18 +154,29 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin 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_ALIGNED_MESSAGE(msg, 32); - msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[32]; + _mav_put_uint64_t(buf, 0, usec); + _mav_put_float(buf, 8, accel_0); + _mav_put_float(buf, 12, accel_1); + _mav_put_float(buf, 16, accel_2); + _mav_put_float(buf, 20, gyro_0); + _mav_put_float(buf, 24, gyro_1); + _mav_put_float(buf, 28, gyro_2); - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds) - put_float_by_index(msg, 8, accel_0); // b_f[0] - put_float_by_index(msg, 12, accel_1); // b_f[1] - put_float_by_index(msg, 16, accel_2); // b_f[2] - put_float_by_index(msg, 20, gyro_0); // b_f[0] - put_float_by_index(msg, 24, gyro_1); // b_f[1] - put_float_by_index(msg, 28, gyro_2); // b_f[2] + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS, buf, 32, 34); +#else + mavlink_nav_filter_bias_t packet; + packet.usec = usec; + packet.accel_0 = accel_0; + packet.accel_1 = accel_1; + packet.accel_2 = accel_2; + packet.gyro_0 = gyro_0; + packet.gyro_1 = gyro_1; + packet.gyro_2 = gyro_2; - mavlink_finalize_message_chan_send(msg, chan, 32, 34); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS, (const char *)&packet, 32, 34); +#endif } #endif @@ -150,7 +191,7 @@ 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) { - return MAVLINK_MSG_RETURN_uint64_t(msg, 0); + return _MAV_RETURN_uint64_t(msg, 0); } /** @@ -160,7 +201,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 8); + return _MAV_RETURN_float(msg, 8); } /** @@ -170,7 +211,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 12); + return _MAV_RETURN_float(msg, 12); } /** @@ -180,7 +221,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 16); + return _MAV_RETURN_float(msg, 16); } /** @@ -190,7 +231,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 20); + return _MAV_RETURN_float(msg, 20); } /** @@ -200,7 +241,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 24); + return _MAV_RETURN_float(msg, 24); } /** @@ -210,7 +251,7 @@ 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) { - return MAVLINK_MSG_RETURN_float(msg, 28); + return _MAV_RETURN_float(msg, 28); } /** @@ -230,6 +271,6 @@ static inline void mavlink_msg_nav_filter_bias_decode(const mavlink_message_t* m 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); #else - memcpy(nav_filter_bias, MAVLINK_PAYLOAD(msg), 32); + memcpy(nav_filter_bias, _MAV_PAYLOAD(msg), 32); #endif } diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index aa46269c6..3ca3010b7 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -52,15 +52,29 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; + + _mav_put_uint16_t_array(buf, 0, aileron, 3); + _mav_put_uint16_t_array(buf, 6, elevator, 3); + _mav_put_uint16_t_array(buf, 12, rudder, 3); + _mav_put_uint16_t_array(buf, 18, gyro, 2); + _mav_put_uint16_t_array(buf, 22, pitch, 5); + _mav_put_uint16_t_array(buf, 32, throttle, 5); + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_radio_calibration_t packet; - put_uint16_t_array_by_index(msg, 0, aileron, 3); // Aileron setpoints: left, center, right - put_uint16_t_array_by_index(msg, 6, elevator, 3); // Elevator setpoints: nose down, center, nose up - put_uint16_t_array_by_index(msg, 12, rudder, 3); // Rudder setpoints: nose left, center, nose right - put_uint16_t_array_by_index(msg, 18, gyro, 2); // Tail gyro mode/gain setpoints: heading hold, rate mode - put_uint16_t_array_by_index(msg, 22, pitch, 5); // Pitch curve setpoints (every 25%) - put_uint16_t_array_by_index(msg, 32, throttle, 5); // Throttle curve setpoints (every 25%) + memcpy(packet.aileron, aileron, sizeof(uint16_t)*3); + memcpy(packet.elevator, elevator, sizeof(uint16_t)*3); + memcpy(packet.rudder, rudder, sizeof(uint16_t)*3); + memcpy(packet.gyro, gyro, sizeof(uint16_t)*2); + memcpy(packet.pitch, pitch, sizeof(uint16_t)*5); + memcpy(packet.throttle, throttle, sizeof(uint16_t)*5); + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; return mavlink_finalize_message(msg, system_id, component_id, 42, 71); } @@ -82,15 +96,29 @@ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_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) { - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; - put_uint16_t_array_by_index(msg, 0, aileron, 3); // Aileron setpoints: left, center, right - put_uint16_t_array_by_index(msg, 6, elevator, 3); // Elevator setpoints: nose down, center, nose up - put_uint16_t_array_by_index(msg, 12, rudder, 3); // Rudder setpoints: nose left, center, nose right - put_uint16_t_array_by_index(msg, 18, gyro, 2); // Tail gyro mode/gain setpoints: heading hold, rate mode - put_uint16_t_array_by_index(msg, 22, pitch, 5); // Pitch curve setpoints (every 25%) - put_uint16_t_array_by_index(msg, 32, throttle, 5); // Throttle curve setpoints (every 25%) + _mav_put_uint16_t_array(buf, 0, aileron, 3); + _mav_put_uint16_t_array(buf, 6, elevator, 3); + _mav_put_uint16_t_array(buf, 12, rudder, 3); + _mav_put_uint16_t_array(buf, 18, gyro, 2); + _mav_put_uint16_t_array(buf, 22, pitch, 5); + _mav_put_uint16_t_array(buf, 32, throttle, 5); + memcpy(_MAV_PAYLOAD(msg), buf, 42); +#else + mavlink_radio_calibration_t packet; + memcpy(packet.aileron, aileron, sizeof(uint16_t)*3); + memcpy(packet.elevator, elevator, sizeof(uint16_t)*3); + memcpy(packet.rudder, rudder, sizeof(uint16_t)*3); + memcpy(packet.gyro, gyro, sizeof(uint16_t)*2); + memcpy(packet.pitch, pitch, sizeof(uint16_t)*5); + memcpy(packet.throttle, throttle, sizeof(uint16_t)*5); + memcpy(_MAV_PAYLOAD(msg), &packet, 42); +#endif + + msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 71); } @@ -122,17 +150,27 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u 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_ALIGNED_MESSAGE(msg, 42); - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[42]; - put_uint16_t_array_by_index(msg, 0, aileron, 3); // Aileron setpoints: left, center, right - put_uint16_t_array_by_index(msg, 6, elevator, 3); // Elevator setpoints: nose down, center, nose up - put_uint16_t_array_by_index(msg, 12, rudder, 3); // Rudder setpoints: nose left, center, nose right - put_uint16_t_array_by_index(msg, 18, gyro, 2); // Tail gyro mode/gain setpoints: heading hold, rate mode - put_uint16_t_array_by_index(msg, 22, pitch, 5); // Pitch curve setpoints (every 25%) - put_uint16_t_array_by_index(msg, 32, throttle, 5); // Throttle curve setpoints (every 25%) + _mav_put_uint16_t_array(buf, 0, aileron, 3); + _mav_put_uint16_t_array(buf, 6, elevator, 3); + _mav_put_uint16_t_array(buf, 12, rudder, 3); + _mav_put_uint16_t_array(buf, 18, gyro, 2); + _mav_put_uint16_t_array(buf, 22, pitch, 5); + _mav_put_uint16_t_array(buf, 32, throttle, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_CALIBRATION, buf, 42, 71); +#else + mavlink_radio_calibration_t packet; - mavlink_finalize_message_chan_send(msg, chan, 42, 71); + memcpy(packet.aileron, aileron, sizeof(uint16_t)*3); + memcpy(packet.elevator, elevator, sizeof(uint16_t)*3); + memcpy(packet.rudder, rudder, sizeof(uint16_t)*3); + memcpy(packet.gyro, gyro, sizeof(uint16_t)*2); + memcpy(packet.pitch, pitch, sizeof(uint16_t)*5); + memcpy(packet.throttle, throttle, sizeof(uint16_t)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RADIO_CALIBRATION, (const char *)&packet, 42, 71); +#endif } #endif @@ -147,7 +185,7 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co */ static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t *aileron) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, aileron, 3, 0); + return _MAV_RETURN_uint16_t_array(msg, aileron, 3, 0); } /** @@ -157,7 +195,7 @@ static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_m */ static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t *elevator) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, elevator, 3, 6); + return _MAV_RETURN_uint16_t_array(msg, elevator, 3, 6); } /** @@ -167,7 +205,7 @@ static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_ */ static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t *rudder) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, rudder, 3, 12); + return _MAV_RETURN_uint16_t_array(msg, rudder, 3, 12); } /** @@ -177,7 +215,7 @@ static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_me */ static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t *gyro) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, gyro, 2, 18); + return _MAV_RETURN_uint16_t_array(msg, gyro, 2, 18); } /** @@ -187,7 +225,7 @@ static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_mess */ static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t *pitch) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, pitch, 5, 22); + return _MAV_RETURN_uint16_t_array(msg, pitch, 5, 22); } /** @@ -197,7 +235,7 @@ static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_mes */ static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t *throttle) { - return MAVLINK_MSG_RETURN_uint16_t_array(msg, throttle, 5, 32); + return _MAV_RETURN_uint16_t_array(msg, throttle, 5, 32); } /** @@ -216,6 +254,6 @@ static inline void mavlink_msg_radio_calibration_decode(const mavlink_message_t* mavlink_msg_radio_calibration_get_pitch(msg, radio_calibration->pitch); mavlink_msg_radio_calibration_get_throttle(msg, radio_calibration->throttle); #else - memcpy(radio_calibration, MAVLINK_PAYLOAD(msg), 42); + memcpy(radio_calibration, _MAV_PAYLOAD(msg), 42); #endif } 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 d6dfe47f1..fe0d9169a 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -38,12 +38,23 @@ 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) { - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, mode); + _mav_put_uint8_t(buf, 1, nav_mode); + _mav_put_uint8_t(buf, 2, pilot); + + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_ualberta_sys_status_t packet; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.pilot = pilot; - put_uint8_t_by_index(msg, 0, mode); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - put_uint8_t_by_index(msg, 1, nav_mode); // Navigation mode, see UALBERTA_NAV_MODE ENUM - put_uint8_t_by_index(msg, 2, pilot); // Pilot mode, see UALBERTA_PILOT_MODE + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; return mavlink_finalize_message(msg, system_id, component_id, 3, 15); } @@ -62,12 +73,23 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_ mavlink_message_t* msg, uint8_t mode,uint8_t nav_mode,uint8_t pilot) { - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, mode); + _mav_put_uint8_t(buf, 1, nav_mode); + _mav_put_uint8_t(buf, 2, pilot); - put_uint8_t_by_index(msg, 0, mode); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - put_uint8_t_by_index(msg, 1, nav_mode); // Navigation mode, see UALBERTA_NAV_MODE ENUM - put_uint8_t_by_index(msg, 2, pilot); // Pilot mode, see UALBERTA_PILOT_MODE + memcpy(_MAV_PAYLOAD(msg), buf, 3); +#else + mavlink_ualberta_sys_status_t packet; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.pilot = pilot; + memcpy(_MAV_PAYLOAD(msg), &packet, 3); +#endif + + msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 15); } @@ -96,14 +118,21 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { - MAVLINK_ALIGNED_MESSAGE(msg, 3); - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[3]; + _mav_put_uint8_t(buf, 0, mode); + _mav_put_uint8_t(buf, 1, nav_mode); + _mav_put_uint8_t(buf, 2, pilot); - put_uint8_t_by_index(msg, 0, mode); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM - put_uint8_t_by_index(msg, 1, nav_mode); // Navigation mode, see UALBERTA_NAV_MODE ENUM - put_uint8_t_by_index(msg, 2, pilot); // Pilot mode, see UALBERTA_PILOT_MODE + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS, buf, 3, 15); +#else + mavlink_ualberta_sys_status_t packet; + packet.mode = mode; + packet.nav_mode = nav_mode; + packet.pilot = pilot; - mavlink_finalize_message_chan_send(msg, chan, 3, 15); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS, (const char *)&packet, 3, 15); +#endif } #endif @@ -118,7 +147,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 0); + return _MAV_RETURN_uint8_t(msg, 0); } /** @@ -128,7 +157,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 1); + return _MAV_RETURN_uint8_t(msg, 1); } /** @@ -138,7 +167,7 @@ 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 MAVLINK_MSG_RETURN_uint8_t(msg, 2); + return _MAV_RETURN_uint8_t(msg, 2); } /** @@ -154,6 +183,6 @@ static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_ 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); #else - memcpy(ualberta_sys_status, MAVLINK_PAYLOAD(msg), 3); + memcpy(ualberta_sys_status, _MAV_PAYLOAD(msg), 3); #endif } diff --git a/thirdParty/mavlink/include/ualberta/testsuite.h b/thirdParty/mavlink/include/ualberta/testsuite.h index 94ff8fc53..11ba27a3c 100644 --- a/thirdParty/mavlink/include/ualberta/testsuite.h +++ b/thirdParty/mavlink/include/ualberta/testsuite.h @@ -11,25 +11,25 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t); -static void mavlink_test_ualberta(uint8_t, uint8_t); +static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); +static void mavlink_test_ualberta(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_all(uint8_t system_id, uint8_t component_id) +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - mavlink_test_common(system_id, component_id); - mavlink_test_ualberta(system_id, component_id); + mavlink_test_common(system_id, component_id, last_msg); + mavlink_test_ualberta(system_id, component_id, last_msg); } #endif #include "../common/testsuite.h" -static void mavlink_test_nav_filter_bias(uint8_t system_id, uint8_t component_id) +static void mavlink_test_nav_filter_bias(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_nav_filter_bias_t packet2, packet1 = { + mavlink_nav_filter_bias_t packet_in = { 93372036854775807ULL, 73.0, 101.0, @@ -38,27 +38,53 @@ static void mavlink_test_nav_filter_bias(uint8_t system_id, uint8_t component_id 185.0, 213.0, }; - if (sizeof(packet2) != 32) { - packet2 = packet1; // cope with alignment within the packet - } + mavlink_nav_filter_bias_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.usec = packet_in.usec; + packet1.accel_0 = packet_in.accel_0; + packet1.accel_1 = packet_in.accel_1; + packet1.accel_2 = packet_in.accel_2; + packet1.gyro_0 = packet_in.gyro_0; + packet1.gyro_1 = packet_in.gyro_1; + packet1.gyro_2 = packet_in.gyro_2; + + + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_nav_filter_bias_encode(system_id, component_id, &msg, &packet1); mavlink_msg_nav_filter_bias_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_nav_filter_bias_pack(system_id, component_id, &msg , packet1.usec , packet1.accel_0 , packet1.accel_1 , packet1.accel_2 , packet1.gyro_0 , packet1.gyro_1 , packet1.gyro_2 ); + mavlink_msg_nav_filter_bias_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_nav_filter_bias_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.accel_0 , packet1.accel_1 , packet1.accel_2 , packet1.gyro_0 , packet1.gyro_1 , packet1.gyro_2 ); + mavlink_msg_nav_filter_bias_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); mavlink_msg_to_send_buffer(buffer, &msg); for (i=0; i>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/ualberta/version.h b/thirdParty/mavlink/include/ualberta/version.h index 65adf37c2..7d09e43df 100644 --- a/thirdParty/mavlink/include/ualberta/version.h +++ b/thirdParty/mavlink/include/ualberta/version.h @@ -5,7 +5,11 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H +<<<<<<< HEAD #define MAVLINK_BUILD_DATE "Mon Aug 29 10:39:58 2011" +======= +#define MAVLINK_BUILD_DATE "Wed Aug 31 18:15:08 2011" +>>>>>>> 720cd74fe31475906a3a527f70f564b1ebc17eb5 #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #include "mavlink.h" diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index d8de138c1..c469353c6 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -711,7 +711,8 @@ Battery voltage, in millivolts (1 = 1 millivolt) Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current Watts consumed from this battery since startup - Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery + Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery + Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) Autopilot-specific errors Autopilot-specific errors -- 2.22.0