From 2977b93b42cf1560c395ae4dba674f8b164e8bc1 Mon Sep 17 00:00:00 2001 From: LM Date: Mon, 29 Aug 2011 10:58:03 +0200 Subject: [PATCH] Updated MAVLink to latest draft --- .../include/ardupilotmega/ardupilotmega.h | 2 +- .../mavlink/include/ardupilotmega/mavlink.h | 4 + .../mavlink_msg_sensor_offsets.h | 60 +- .../mavlink_msg_set_mag_offsets.h | 39 +- .../mavlink/include/ardupilotmega/testsuite.h | 6 +- .../mavlink/include/ardupilotmega/version.h | 2 +- thirdParty/mavlink/include/checksum.h | 2 +- thirdParty/mavlink/include/common/common.h | 8 +- thirdParty/mavlink/include/common/mavlink.h | 5 +- .../include/common/mavlink_msg_attitude.h | 45 +- .../include/common/mavlink_msg_auth_key.h | 27 +- .../mavlink_msg_change_operator_control.h | 36 +- .../mavlink_msg_change_operator_control_ack.h | 33 +- .../include/common/mavlink_msg_command_ack.h | 30 +- .../include/common/mavlink_msg_command_long.h | 57 +- .../common/mavlink_msg_command_short.h | 48 +- .../include/common/mavlink_msg_data_stream.h | 33 +- .../include/common/mavlink_msg_debug.h | 30 +- .../include/common/mavlink_msg_debug_vect.h | 39 +- .../common/mavlink_msg_global_position.h | 45 +- .../common/mavlink_msg_global_position_int.h | 45 +- ...mavlink_msg_global_position_setpoint_int.h | 36 +- .../common/mavlink_msg_gps_local_origin_set.h | 33 +- .../include/common/mavlink_msg_gps_raw.h | 54 +- .../include/common/mavlink_msg_gps_raw_int.h | 54 +- .../mavlink_msg_gps_set_global_origin.h | 39 +- .../include/common/mavlink_msg_gps_status.h | 42 +- .../include/common/mavlink_msg_heartbeat.h | 47 +- .../include/common/mavlink_msg_hil_controls.h | 57 +- .../common/mavlink_msg_hil_rc_inputs_raw.h | 66 +- .../include/common/mavlink_msg_hil_state.h | 70 +- .../common/mavlink_msg_local_position.h | 45 +- .../mavlink_msg_local_position_setpoint.h | 36 +- .../mavlink_msg_local_position_setpoint_set.h | 42 +- .../common/mavlink_msg_manual_control.h | 51 +- .../include/common/mavlink_msg_memory_vect.h | 36 +- .../common/mavlink_msg_named_value_float.h | 30 +- .../common/mavlink_msg_named_value_int.h | 30 +- .../mavlink_msg_nav_controller_output.h | 48 +- .../include/common/mavlink_msg_optical_flow.h | 42 +- .../common/mavlink_msg_param_request_list.h | 30 +- .../common/mavlink_msg_param_request_read.h | 36 +- .../include/common/mavlink_msg_param_set.h | 39 +- .../include/common/mavlink_msg_param_value.h | 39 +- .../mavlink/include/common/mavlink_msg_ping.h | 36 +- .../common/mavlink_msg_position_target.h | 36 +- .../include/common/mavlink_msg_raw_imu.h | 54 +- .../include/common/mavlink_msg_raw_pressure.h | 39 +- .../common/mavlink_msg_rc_channels_override.h | 54 +- .../common/mavlink_msg_rc_channels_raw.h | 51 +- .../common/mavlink_msg_rc_channels_scaled.h | 51 +- .../common/mavlink_msg_request_data_stream.h | 39 +- ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 39 +- ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 39 +- .../common/mavlink_msg_safety_allowed_area.h | 45 +- .../mavlink_msg_safety_set_allowed_area.h | 51 +- .../include/common/mavlink_msg_scaled_imu.h | 54 +- .../common/mavlink_msg_scaled_pressure.h | 36 +- .../common/mavlink_msg_servo_output_raw.h | 48 +- .../common/mavlink_msg_set_flight_mode.h | 30 +- .../include/common/mavlink_msg_set_mode.h | 30 +- ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 42 +- .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 42 +- .../common/mavlink_msg_set_safety_mode.h | 30 +- .../common/mavlink_msg_state_correction.h | 51 +- .../include/common/mavlink_msg_statustext.h | 30 +- .../include/common/mavlink_msg_sys_status.h | 60 +- .../include/common/mavlink_msg_system_time.h | 30 +- .../common/mavlink_msg_system_time_utc.h | 30 +- .../include/common/mavlink_msg_vfr_hud.h | 42 +- .../include/common/mavlink_msg_waypoint.h | 66 +- .../include/common/mavlink_msg_waypoint_ack.h | 33 +- .../common/mavlink_msg_waypoint_clear_all.h | 30 +- .../common/mavlink_msg_waypoint_count.h | 33 +- .../common/mavlink_msg_waypoint_current.h | 27 +- .../common/mavlink_msg_waypoint_reached.h | 27 +- .../common/mavlink_msg_waypoint_request.h | 33 +- .../mavlink_msg_waypoint_request_list.h | 30 +- .../common/mavlink_msg_waypoint_set_current.h | 33 +- thirdParty/mavlink/include/common/testsuite.h | 210 +-- thirdParty/mavlink/include/common/version.h | 2 +- thirdParty/mavlink/include/mavlink_helpers.h | 28 +- thirdParty/mavlink/include/minimal/mavlink.h | 4 + .../include/minimal/mavlink_msg_heartbeat.h | 47 +- .../mavlink/include/minimal/testsuite.h | 3 +- thirdParty/mavlink/include/minimal/version.h | 2 +- thirdParty/mavlink/include/pixhawk/mavlink.h | 4 + .../pixhawk/mavlink_msg_attitude_control.h | 51 +- .../pixhawk/mavlink_msg_brief_feature.h | 48 +- .../mavlink_msg_data_transmission_handshake.h | 39 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 30 +- .../pixhawk/mavlink_msg_image_available.h | 77 +- .../mavlink_msg_image_trigger_control.h | 27 +- .../pixhawk/mavlink_msg_image_triggered.h | 60 +- .../include/pixhawk/mavlink_msg_marker.h | 45 +- .../pixhawk/mavlink_msg_pattern_detected.h | 36 +- .../pixhawk/mavlink_msg_point_of_interest.h | 48 +- ...mavlink_msg_point_of_interest_connection.h | 57 +- .../mavlink_msg_position_control_offset_set.h | 42 +- .../mavlink_msg_position_control_setpoint.h | 39 +- ...avlink_msg_position_control_setpoint_set.h | 45 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 45 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 42 +- .../mavlink_msg_vicon_position_estimate.h | 45 +- .../mavlink_msg_vision_position_estimate.h | 45 +- .../mavlink_msg_vision_speed_estimate.h | 36 +- .../pixhawk/mavlink_msg_watchdog_command.h | 36 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 30 +- .../mavlink_msg_watchdog_process_info.h | 39 +- .../mavlink_msg_watchdog_process_status.h | 42 +- .../mavlink/include/pixhawk/testsuite.h | 69 +- thirdParty/mavlink/include/pixhawk/version.h | 2 +- thirdParty/mavlink/include/protocol.h | 431 ++--- thirdParty/mavlink/include/slugs/mavlink.h | 4 + .../include/slugs/mavlink_msg_air_data.h | 33 +- .../include/slugs/mavlink_msg_cpu_load.h | 33 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 30 +- .../include/slugs/mavlink_msg_data_log.h | 42 +- .../include/slugs/mavlink_msg_diagnostic.h | 42 +- .../include/slugs/mavlink_msg_gps_date_time.h | 45 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 36 +- .../include/slugs/mavlink_msg_sensor_bias.h | 42 +- .../include/slugs/mavlink_msg_slugs_action.h | 33 +- .../slugs/mavlink_msg_slugs_navigation.h | 51 +- thirdParty/mavlink/include/slugs/testsuite.h | 30 +- thirdParty/mavlink/include/slugs/version.h | 2 +- thirdParty/mavlink/include/test/mavlink.h | 4 + .../include/test/mavlink_msg_test_types.h | 76 +- thirdParty/mavlink/include/test/testsuite.h | 3 +- thirdParty/mavlink/include/test/version.h | 2 +- thirdParty/mavlink/include/ualberta/mavlink.h | 4 + .../ualberta/mavlink_msg_nav_filter_bias.h | 45 +- .../ualberta/mavlink_msg_radio_calibration.h | 42 +- .../mavlink_msg_ualberta_sys_status.h | 33 +- .../mavlink/include/ualberta/testsuite.h | 9 +- thirdParty/mavlink/include/ualberta/version.h | 2 +- .../mavlink/message_definitions/common.xml | 167 +- .../message_definitions/common_future.xml | 1399 +++++++++++++++++ .../missionlib/mavlink_missionlib_data.h | 58 + .../mavlink/missionlib/mavlink_parameters.c | 10 +- thirdParty/mavlink/missionlib/waypoints.c | 28 +- thirdParty/mavlink/missionlib/waypoints.h | 10 +- 142 files changed, 2940 insertions(+), 4180 deletions(-) create mode 100644 thirdParty/mavlink/message_definitions/common_future.xml create mode 100644 thirdParty/mavlink/missionlib/mavlink_missionlib_data.h diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 5e03ee940..099981bf3 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -20,7 +20,7 @@ extern "C" { #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, {}, {}, {}, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_FLIGHT_MODE, MAVLINK_MESSAGE_INFO_SET_SAFETY_MODE, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, {}, MAVLINK_MESSAGE_INFO_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, {}, {}, {}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {}, {}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_SHORT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, 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, {"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} #endif #include "../protocol.h" diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 0f83f1ea0..551938f04 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -13,6 +13,10 @@ #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + #ifndef MAVLINK_CRC_EXTRA #define MAVLINK_CRC_EXTRA 1 #endif diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h index e94e7e8f9..e9cd44297 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h @@ -125,49 +125,6 @@ static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, u return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 134); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a sensor_offsets message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - * @param mag_declination magnetic declination (radians) - * @param raw_press raw pressure from barometer - * @param raw_temp raw temperature from barometer - * @param gyro_cal_x gyro X calibration - * @param gyro_cal_y gyro Y calibration - * @param gyro_cal_z gyro Z calibration - * @param accel_cal_x accel X calibration - * @param accel_cal_y accel Y calibration - * @param accel_cal_z accel Z calibration - */ -static inline void mavlink_msg_sensor_offsets_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z,float mag_declination,int32_t raw_press,int32_t raw_temp,float gyro_cal_x,float gyro_cal_y,float gyro_cal_z,float accel_cal_x,float accel_cal_y,float accel_cal_z) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a sensor_offsets struct into a message * @@ -203,7 +160,22 @@ 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); - mavlink_msg_sensor_offsets_pack_chan_send(chan, msg, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z); + 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); } #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 3e7086f20..3083d7628 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 219); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_mag_offsets message on a channel and send - * @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 mag_ofs_x magnetometer X offset - * @param mag_ofs_y magnetometer Y offset - * @param mag_ofs_z magnetometer Z offset - */ -static inline void mavlink_msg_set_mag_offsets_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t mag_ofs_x,int16_t mag_ofs_y,int16_t mag_ofs_z) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_mag_offsets struct into a message * @@ -140,7 +111,15 @@ 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); - mavlink_msg_set_mag_offsets_pack_chan_send(chan, msg, target_system, target_component, mag_ofs_x, mag_ofs_y, 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 + + mavlink_finalize_message_chan_send(msg, chan, 8, 219); } #endif diff --git a/thirdParty/mavlink/include/ardupilotmega/testsuite.h b/thirdParty/mavlink/include/ardupilotmega/testsuite.h index fa6ef970a..2fd63d85c 100644 --- a/thirdParty/mavlink/include/ardupilotmega/testsuite.h +++ b/thirdParty/mavlink/include/ardupilotmega/testsuite.h @@ -55,8 +55,7 @@ static void mavlink_test_sensor_offsets(uint8_t system_id, uint8_t component_id) for (i=0; imsgid = MAVLINK_MSG_ID_ATTITUDE; - - put_uint64_t_by_index(msg, 0, 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) - - mavlink_finalize_message_chan_send(msg, chan, 32, 66); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a attitude struct into a message * @@ -158,7 +125,17 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { MAVLINK_ALIGNED_MESSAGE(msg, 32); - mavlink_msg_attitude_pack_chan_send(chan, msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed); + msg->msgid = MAVLINK_MSG_ID_ATTITUDE; + + put_uint64_t_by_index(msg, 0, 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) + + mavlink_finalize_message_chan_send(msg, chan, 32, 66); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index ca24dd88f..ad7e8c461 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -59,27 +59,6 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 119); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a auth_key message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param key key - */ -static inline void mavlink_msg_auth_key_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - const char *key) -{ - msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - - put_char_array_by_index(msg, 0, key, 32); // key - - mavlink_finalize_message_chan_send(msg, chan, 32, 119); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a auth_key struct into a message * @@ -104,7 +83,11 @@ 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); - mavlink_msg_auth_key_pack_chan_send(chan, msg, key); + msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; + + put_char_array_by_index(msg, 0, key, 32); // key + + mavlink_finalize_message_chan_send(msg, chan, 32, 119); } #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 47c919f61..29193f472 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 217); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a change_operator_control message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System the GCS requests control for - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - */ -static inline void mavlink_msg_change_operator_control_pack_chan_send(mavlink_channel_t chan, - 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 "!?,.-" - - mavlink_finalize_message_chan_send(msg, chan, 28, 217); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a change_operator_control struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_change_operator_control_pack_chan_send(chan, msg, target_system, control_request, version, 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 "!?,.-" + + mavlink_finalize_message_chan_send(msg, chan, 28, 217); } #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 6cc9ef58a..674820dbb 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 @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a change_operator_control_ack message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param gcs_system_id ID of the GCS this message - * @param control_request 0: request control of this MAV, 1: Release control of this MAV - * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - */ -static inline void mavlink_msg_change_operator_control_ack_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 3, 104); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a change_operator_control_ack struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_change_operator_control_ack_pack_chan_send(chan, msg, gcs_system_id, control_request, ack); + msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_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 + + mavlink_finalize_message_chan_send(msg, chan, 3, 104); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index d2547367f..d74520446 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 8); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a command_ack message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param command Current airspeed in m/s - * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - */ -static inline void mavlink_msg_command_ack_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float command,float result) -{ - msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 8, 8); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a command_ack struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_command_ack_pack_chan_send(chan, msg, command, result); + msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 8, 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 0c498e165..43a05eaa3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_long.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_long.h @@ -119,47 +119,6 @@ static inline uint16_t mavlink_msg_command_long_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 168); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a command_long message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - * @param param5 Parameter 5, as defined by MAV_CMD enum. - * @param param6 Parameter 6, as defined by MAV_CMD enum. - * @param param7 Parameter 7, as defined by MAV_CMD enum. - */ -static inline void mavlink_msg_command_long_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t command,uint8_t confirmation,float param1,float param2,float param3,float param4,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) - - mavlink_finalize_message_chan_send(msg, chan, 32, 168); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a command_long struct into a message * @@ -194,7 +153,21 @@ 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); - mavlink_msg_command_long_pack_chan_send(chan, msg, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, 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) + + mavlink_finalize_message_chan_send(msg, chan, 32, 168); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_short.h b/thirdParty/mavlink/include/common/mavlink_msg_command_short.h index 876e3b82a..7a9a6e62d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_short.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_short.h @@ -101,41 +101,6 @@ static inline uint16_t mavlink_msg_command_short_pack_chan(uint8_t system_id, ui return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 160); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a command_short message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System which should execute the command - * @param target_component Component which should execute the command, 0 for all components - * @param command Command ID, as defined by MAV_CMD enum. - * @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - * @param param1 Parameter 1, as defined by MAV_CMD enum. - * @param param2 Parameter 2, as defined by MAV_CMD enum. - * @param param3 Parameter 3, as defined by MAV_CMD enum. - * @param param4 Parameter 4, as defined by MAV_CMD enum. - */ -static inline void mavlink_msg_command_short_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t command,uint8_t confirmation,float param1,float param2,float param3,float param4) -{ - msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; - - 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) - - mavlink_finalize_message_chan_send(msg, chan, 20, 160); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a command_short struct into a message * @@ -167,7 +132,18 @@ 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); - mavlink_msg_command_short_pack_chan_send(chan, msg, target_system, target_component, command, confirmation, param1, param2, param3, param4); + msg->msgid = MAVLINK_MSG_ID_COMMAND_SHORT; + + 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) + + mavlink_finalize_message_chan_send(msg, chan, 20, 160); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h index 1ff8b9a88..efcb7c9b8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_data_stream.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_data_stream_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 21); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a data_stream message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param stream_id The ID of the requested data stream - * @param message_rate The requested interval between two messages of this type - * @param on_off 1 stream is enabled, 0 stream is stopped. - */ -static inline void mavlink_msg_data_stream_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t stream_id,uint16_t message_rate,uint8_t on_off) -{ - msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; - - 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. - - mavlink_finalize_message_chan_send(msg, chan, 4, 21); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a data_stream struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_data_stream_pack_chan_send(chan, msg, stream_id, message_rate, on_off); + msg->msgid = MAVLINK_MSG_ID_DATA_STREAM; + + 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. + + mavlink_finalize_message_chan_send(msg, chan, 4, 21); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index 5aafb0987..de1d4f41f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 127); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a debug message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param ind index of debug variable - * @param value DEBUG value - */ -static inline void mavlink_msg_debug_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t ind,float value) -{ - msg->msgid = MAVLINK_MSG_ID_DEBUG; - - put_float_by_index(msg, 0, value); // DEBUG value - put_uint8_t_by_index(msg, 4, ind); // index of debug variable - - mavlink_finalize_message_chan_send(msg, chan, 5, 127); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a debug struct into a message * @@ -113,7 +90,12 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { MAVLINK_ALIGNED_MESSAGE(msg, 5); - mavlink_msg_debug_pack_chan_send(chan, msg, ind, value); + msg->msgid = MAVLINK_MSG_ID_DEBUG; + + put_float_by_index(msg, 0, value); // DEBUG value + put_uint8_t_by_index(msg, 4, ind); // index of debug variable + + mavlink_finalize_message_chan_send(msg, chan, 5, 127); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 4fe0d8eb2..a56857f6c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 15); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a debug_vect message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param name Name - * @param usec Timestamp - * @param x x - * @param y y - * @param z z - */ -static inline void mavlink_msg_debug_vect_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - const char *name,uint64_t usec,float x,float y,float z) -{ - msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - - put_uint64_t_by_index(msg, 0, 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, 15); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a debug_vect struct into a message * @@ -140,7 +111,15 @@ 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 usec, float x, float y, float z) { MAVLINK_ALIGNED_MESSAGE(msg, 30); - mavlink_msg_debug_vect_pack_chan_send(chan, msg, name, usec, x, y, z); + msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; + + put_uint64_t_by_index(msg, 0, 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, 15); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index 6f99c355b..1c43267f5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 147); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a global_position message on a channel and send - * @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) - */ -static inline void mavlink_msg_global_position_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t usec,float lat,float lon,float alt,float vx,float vy,float vz) -{ - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since unix epoch) - put_float_by_index(msg, 8, lat); // Latitude, in degrees - put_float_by_index(msg, 12, lon); // Longitude, in degrees - put_float_by_index(msg, 16, alt); // Absolute altitude, in meters - put_float_by_index(msg, 20, vx); // X Speed (in Latitude direction, positive: going north) - put_float_by_index(msg, 24, vy); // Y Speed (in Longitude direction, positive: going east) - put_float_by_index(msg, 28, vz); // Z Speed (in Altitude direction, positive: going up) - - mavlink_finalize_message_chan_send(msg, chan, 32, 147); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a global_position struct into a message * @@ -158,7 +125,17 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { MAVLINK_ALIGNED_MESSAGE(msg, 32); - mavlink_msg_global_position_pack_chan_send(chan, msg, usec, lat, lon, alt, vx, vy, vz); + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since unix epoch) + put_float_by_index(msg, 8, lat); // Latitude, in degrees + put_float_by_index(msg, 12, lon); // Longitude, in degrees + put_float_by_index(msg, 16, alt); // Absolute altitude, in meters + put_float_by_index(msg, 20, vx); // X Speed (in Latitude direction, positive: going north) + put_float_by_index(msg, 24, vy); // Y Speed (in Longitude direction, positive: going east) + put_float_by_index(msg, 28, vz); // Z Speed (in Altitude direction, positive: going up) + + mavlink_finalize_message_chan_send(msg, chan, 32, 147); } #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 6edfa952a..3dccd40f5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 241); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a global_position_int message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters), above MSL - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - */ -static inline void mavlink_msg_global_position_int_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - int32_t lat,int32_t lon,int32_t alt,int16_t vx,int16_t vy,int16_t vz,uint16_t hdg) -{ - msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - - put_int32_t_by_index(msg, 0, lat); // Latitude, expressed as * 1E7 - put_int32_t_by_index(msg, 4, lon); // Longitude, expressed as * 1E7 - put_int32_t_by_index(msg, 8, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL - put_int16_t_by_index(msg, 12, vx); // Ground X Speed (Latitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 14, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 - put_int16_t_by_index(msg, 16, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 - put_uint16_t_by_index(msg, 18, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - - mavlink_finalize_message_chan_send(msg, chan, 20, 241); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a global_position_int struct into a message * @@ -158,7 +125,17 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { MAVLINK_ALIGNED_MESSAGE(msg, 20); - mavlink_msg_global_position_int_pack_chan_send(chan, msg, lat, lon, alt, vx, vy, vz, hdg); + msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + + put_int32_t_by_index(msg, 0, lat); // Latitude, expressed as * 1E7 + put_int32_t_by_index(msg, 4, lon); // Longitude, expressed as * 1E7 + put_int32_t_by_index(msg, 8, alt); // Altitude in meters, expressed as * 1000 (millimeters), above MSL + put_int16_t_by_index(msg, 12, vx); // Ground X Speed (Latitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 14, vy); // Ground Y Speed (Longitude), expressed as m/s * 100 + put_int16_t_by_index(msg, 16, vz); // Ground Z Speed (Altitude), expressed as m/s * 100 + put_uint16_t_by_index(msg, 18, hdg); // Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + + mavlink_finalize_message_chan_send(msg, chan, 20, 241); } #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 8788ad520..3f90f5491 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 @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_global_position_setpoint_int_pack_chan(uint8_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 142); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a global_position_setpoint_int message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param latitude WGS84 Latitude position in degrees * 1E7 - * @param longitude WGS84 Longitude position in degrees * 1E7 - * @param altitude WGS84 Altitude in meters * 1000 (positive for up) - * @param yaw Desired yaw angle in degrees * 100 - */ -static inline void mavlink_msg_global_position_setpoint_int_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 14, 142); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a global_position_setpoint_int struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_global_position_setpoint_int_pack_chan_send(chan, msg, latitude, longitude, altitude, yaw); + 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 + + mavlink_finalize_message_chan_send(msg, chan, 14, 142); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h index 4365fa82b..2ee9d0c09 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack_chan(uint8_t system return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 14); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a gps_local_origin_set message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param latitude Latitude (WGS84), expressed as * 1E7 - * @param longitude Longitude (WGS84), expressed as * 1E7 - * @param altitude Altitude(WGS84), expressed as * 1000 - */ -static inline void mavlink_msg_gps_local_origin_set_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude) -{ - msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 12, 14); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a gps_local_origin_set struct into a message * @@ -122,7 +97,13 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { MAVLINK_ALIGNED_MESSAGE(msg, 12); - mavlink_msg_gps_local_origin_set_pack_chan_send(chan, msg, latitude, longitude, altitude); + msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 12, 14); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 223056e8c..890664152 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -113,45 +113,6 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 38, 198); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a gps_raw message on a channel and send - * @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 - */ -static inline void mavlink_msg_gps_raw_pack_chan_send(mavlink_channel_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) -{ - msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, lat); // Latitude in degrees - put_float_by_index(msg, 12, lon); // Longitude in degrees - put_float_by_index(msg, 16, alt); // Altitude in meters - put_float_by_index(msg, 20, eph); // GPS HDOP - put_float_by_index(msg, 24, epv); // GPS VDOP - put_float_by_index(msg, 28, v); // GPS ground speed - put_float_by_index(msg, 32, hdg); // Compass heading in degrees, 0..360 degrees - put_uint8_t_by_index(msg, 36, 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, 37, satellites_visible); // Number of satellites visible - - mavlink_finalize_message_chan_send(msg, chan, 38, 198); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a gps_raw struct into a message * @@ -185,7 +146,20 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { MAVLINK_ALIGNED_MESSAGE(msg, 38); - mavlink_msg_gps_raw_pack_chan_send(chan, msg, usec, fix_type, lat, lon, alt, eph, epv, v, hdg, satellites_visible); + msg->msgid = MAVLINK_MSG_ID_GPS_RAW; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, lat); // Latitude in degrees + put_float_by_index(msg, 12, lon); // Longitude in degrees + put_float_by_index(msg, 16, alt); // Altitude in meters + put_float_by_index(msg, 20, eph); // GPS HDOP + put_float_by_index(msg, 24, epv); // GPS VDOP + put_float_by_index(msg, 28, v); // GPS ground speed + put_float_by_index(msg, 32, hdg); // Compass heading in degrees, 0..360 degrees + put_uint8_t_by_index(msg, 36, 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, 37, satellites_visible); // Number of satellites visible + + mavlink_finalize_message_chan_send(msg, chan, 38, 198); } #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 7cac3537b..ff6dda30d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -113,45 +113,6 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 185); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a gps_raw_int message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - * @param lat Latitude in 1E7 degrees - * @param lon Longitude in 1E7 degrees - * @param alt Altitude in 1E3 meters (millimeters) above MSL - * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - * @param satellites_visible Number of satellites visible. If unknown, set to 255 - */ -static inline void mavlink_msg_gps_raw_int_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t 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 hdg,uint8_t satellites_visible) -{ - msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - - put_uint64_t_by_index(msg, 0, 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, hdg); // Compass heading 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, 185); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a gps_raw_int struct into a message * @@ -185,7 +146,20 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { MAVLINK_ALIGNED_MESSAGE(msg, 30); - mavlink_msg_gps_raw_int_pack_chan_send(chan, msg, usec, fix_type, lat, lon, alt, eph, epv, vel, hdg, satellites_visible); + msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; + + put_uint64_t_by_index(msg, 0, 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, hdg); // Compass heading 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, 185); } #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 index cd6b0a3f0..80ec0749f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t syste return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 170); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a gps_set_global_origin message on a channel and send - * @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 - */ -static inline void mavlink_msg_gps_set_global_origin_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int32_t latitude,int32_t longitude,int32_t altitude) -{ - msg->msgid = MAVLINK_MSG_ID_GPS_SET_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 - put_uint8_t_by_index(msg, 13, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 14, 170); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a gps_set_global_origin struct into a message * @@ -140,7 +111,15 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { MAVLINK_ALIGNED_MESSAGE(msg, 14); - mavlink_msg_gps_set_global_origin_pack_chan_send(chan, msg, target_system, target_component, latitude, longitude, altitude); + msg->msgid = MAVLINK_MSG_ID_GPS_SET_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 + put_uint8_t_by_index(msg, 13, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 14, 170); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index ef8f96c8b..592cc24dd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -93,37 +93,6 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 101, 23); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a gps_status message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param satellites_visible Number of satellites visible - * @param satellite_prn Global satellite ID - * @param satellite_used 0: Satellite not used, 1: used for localization - * @param satellite_elevation Elevation (0: right on top of receiver, 90: on the horizon) of satellite - * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. - * @param satellite_snr Signal to noise ratio of satellite - */ -static inline void mavlink_msg_gps_status_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t satellites_visible,const uint8_t *satellite_prn,const uint8_t *satellite_used,const uint8_t *satellite_elevation,const uint8_t *satellite_azimuth,const uint8_t *satellite_snr) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a gps_status struct into a message * @@ -153,7 +122,16 @@ 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); - mavlink_msg_gps_status_pack_chan_send(chan, msg, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, 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 + + mavlink_finalize_message_chan_send(msg, chan, 101, 23); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 19bc0a34a..661868f5e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -99,40 +99,6 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 153); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a heartbeat message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_CLASS ENUM - * @param system_mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - * @param flight_mode Navigation mode, see MAV_FLIGHT_MODE ENUM - * @param system_status System status flag, see MAV_STATUS ENUM - * @param safety_status System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - * @param 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 - */ -static inline void mavlink_msg_heartbeat_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t autopilot,uint8_t system_mode,uint8_t flight_mode,uint8_t system_status,uint8_t safety_status,uint8_t link_status) -{ - msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - - 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, system_mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - put_uint8_t_by_index(msg, 3, flight_mode); // Navigation mode, see MAV_FLIGHT_MODE ENUM - put_uint8_t_by_index(msg, 4, system_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, 3); // MAVLink version - - mavlink_finalize_message_chan_send(msg, chan, 8, 153); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a heartbeat struct into a message * @@ -163,7 +129,18 @@ 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 system_mode, uint8_t flight_mode, uint8_t system_status, uint8_t safety_status, uint8_t link_status) { MAVLINK_ALIGNED_MESSAGE(msg, 8); - mavlink_msg_heartbeat_pack_chan_send(chan, msg, type, autopilot, system_mode, flight_mode, system_status, safety_status, link_status); + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + + 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, system_mode); // System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + put_uint8_t_by_index(msg, 3, flight_mode); // Navigation mode, see MAV_FLIGHT_MODE ENUM + put_uint8_t_by_index(msg, 4, system_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, 3); // MAVLink version + + mavlink_finalize_message_chan_send(msg, chan, 8, 153); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h index 3d5b53f68..f6fd90ca6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h @@ -119,47 +119,6 @@ static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 250); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a hil_controls message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll_ailerons Control output -1 .. 1 - * @param pitch_elevator Control output -1 .. 1 - * @param yaw_rudder Control output -1 .. 1 - * @param throttle Throttle 0 .. 1 - * @param aux1 Aux 1, -1 .. 1 - * @param aux2 Aux 2, -1 .. 1 - * @param aux3 Aux 3, -1 .. 1 - * @param aux4 Aux 4, -1 .. 1 - * @param mode System mode (MAV_MODE) - * @param nav_mode Navigation mode (MAV_NAV_MODE) - */ -static inline void mavlink_msg_hil_controls_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t time_us,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_us); // 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, 250); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a hil_controls struct into a message * @@ -194,7 +153,21 @@ static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, float aux1, float aux2, float aux3, float aux4, uint8_t mode, uint8_t nav_mode) { MAVLINK_ALIGNED_MESSAGE(msg, 42); - mavlink_msg_hil_controls_pack_chan_send(chan, msg, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode); + msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS; + + put_uint64_t_by_index(msg, 0, time_us); // 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, 250); } #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 434c01b8d..3a479dc21 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 @@ -137,53 +137,6 @@ static inline uint16_t mavlink_msg_hil_rc_inputs_raw_pack_chan(uint8_t system_id return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 33, 156); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a hil_rc_inputs_raw message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds - * @param chan9_raw RC channel 9 value, in microseconds - * @param chan10_raw RC channel 10 value, in microseconds - * @param chan11_raw RC channel 11 value, in microseconds - * @param chan12_raw RC channel 12 value, in microseconds - * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% - */ -static inline void mavlink_msg_hil_rc_inputs_raw_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t time_us,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_us); // 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, 156); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a hil_rc_inputs_raw struct into a message * @@ -221,7 +174,24 @@ 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_us, 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); - mavlink_msg_hil_rc_inputs_raw_pack_chan_send(chan, msg, time_us, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi); + msg->msgid = MAVLINK_MSG_ID_HIL_RC_INPUTS_RAW; + + put_uint64_t_by_index(msg, 0, time_us); // 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, 156); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h index baae6d4c9..242531d3e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h @@ -149,12 +149,23 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 56, 12); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - /** - * @brief Pack a hil_state message on a channel and send - * @param chan The MAVLink channel this message was sent over + * @brief Encode a hil_state struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into + * @param hil_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) +{ + return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_us, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); +} + +/** + * @brief Send a hil_state message + * @param chan MAVLink channel to send the message + * * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) * @param roll Roll angle (rad) * @param pitch Pitch angle (rad) @@ -172,10 +183,11 @@ static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_ * @param yacc Y acceleration (mg) * @param zacc Z acceleration (mg) */ -static inline void mavlink_msg_hil_state_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t time_us,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) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_us, 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_us); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) @@ -197,50 +209,6 @@ static inline void mavlink_msg_hil_state_pack_chan_send(mavlink_channel_t chan, mavlink_finalize_message_chan_send(msg, chan, 56, 12); } -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - -/** - * @brief Encode a hil_state struct into a message - * - * @param system_id ID of this system - * @param component_id ID of this component (e.g. 200 for IMU) - * @param msg The MAVLink message to compress the data into - * @param hil_state C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state) -{ - return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->time_us, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc); -} - -/** - * @brief Send a hil_state message - * @param chan MAVLink channel to send the message - * - * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param roll Roll angle (rad) - * @param pitch Pitch angle (rad) - * @param yaw Yaw angle (rad) - * @param rollspeed Roll angular speed (rad/s) - * @param pitchspeed Pitch angular speed (rad/s) - * @param yawspeed Yaw angular speed (rad/s) - * @param lat Latitude, expressed as * 1E7 - * @param lon Longitude, expressed as * 1E7 - * @param alt Altitude in meters, expressed as * 1000 (millimeters) - * @param vx Ground X Speed (Latitude), expressed as m/s * 100 - * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 - * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t time_us, 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); - mavlink_msg_hil_state_pack_chan_send(chan, msg, time_us, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc); -} #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index 69f688d02..15bd6800f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, u return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 126); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a local_position message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param vx X Speed - * @param vy Y Speed - * @param vz Z Speed - */ -static inline void mavlink_msg_local_position_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float vx,float vy,float vz) -{ - msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, x); // X Position - put_float_by_index(msg, 12, y); // Y Position - put_float_by_index(msg, 16, z); // Z Position - put_float_by_index(msg, 20, vx); // X Speed - put_float_by_index(msg, 24, vy); // Y Speed - put_float_by_index(msg, 28, vz); // Z Speed - - mavlink_finalize_message_chan_send(msg, chan, 32, 126); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a local_position struct into a message * @@ -158,7 +125,17 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { MAVLINK_ALIGNED_MESSAGE(msg, 32); - mavlink_msg_local_position_pack_chan_send(chan, msg, usec, x, y, z, vx, vy, vz); + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, x); // X Position + put_float_by_index(msg, 12, y); // Y Position + put_float_by_index(msg, 16, z); // Z Position + put_float_by_index(msg, 20, vx); // X Speed + put_float_by_index(msg, 24, vy); // Y Speed + put_float_by_index(msg, 28, vz); // Z Speed + + mavlink_finalize_message_chan_send(msg, chan, 32, 126); } #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 814f13b69..735b1420c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 50); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a local_position_setpoint message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -static inline void mavlink_msg_local_position_setpoint_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float x,float y,float z,float yaw) -{ - 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 - - mavlink_finalize_message_chan_send(msg, chan, 16, 50); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a local_position_setpoint struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_local_position_setpoint_pack_chan_send(chan, msg, x, y, z, yaw); + 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 + + mavlink_finalize_message_chan_send(msg, chan, 16, 50); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h index 15a56ee66..c09e862dc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack_chan(uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 73); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a local_position_setpoint_set message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param x x position - * @param y y position - * @param z z position - * @param yaw Desired yaw angle - */ -static inline void mavlink_msg_local_position_setpoint_set_pack_chan_send(mavlink_channel_t chan, - 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_LOCAL_POSITION_SETPOINT_SET; - - 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, 73); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a local_position_setpoint_set struct into a message * @@ -149,7 +118,16 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { MAVLINK_ALIGNED_MESSAGE(msg, 18); - mavlink_msg_local_position_setpoint_set_pack_chan_send(chan, msg, target_system, target_component, x, y, z, yaw); + msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; + + 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, 73); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index 7f4a25550..e0e88e05a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -107,43 +107,6 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 21, 52); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a manual_control message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system to be controlled - * @param roll roll - * @param pitch pitch - * @param yaw yaw - * @param thrust thrust - * @param roll_manual roll control enabled auto:0, manual:1 - * @param pitch_manual pitch auto:0, manual:1 - * @param yaw_manual yaw auto:0, manual:1 - * @param thrust_manual thrust auto:0, manual:1 - */ -static inline void mavlink_msg_manual_control_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,float roll,float pitch,float yaw,float thrust,uint8_t roll_manual,uint8_t pitch_manual,uint8_t yaw_manual,uint8_t thrust_manual) -{ - msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a manual_control struct into a message * @@ -176,7 +139,19 @@ 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); - mavlink_msg_manual_control_pack_chan_send(chan, msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); + msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; + + 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); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h index b4b324073..c47f2f929 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 204); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a memory_vect message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param address Starting address of the debug variables - * @param ver Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - * @param 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 - * @param value Memory contents at specified address - */ -static inline void mavlink_msg_memory_vect_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 36, 204); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a memory_vect struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_memory_vect_pack_chan_send(chan, msg, address, ver, type, 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 + + mavlink_finalize_message_chan_send(msg, chan, 36, 204); } #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 6ffb734ca..733b6bfd1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 248); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a named_value_float message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param name Name of the debug variable - * @param value Floating point value - */ -static inline void mavlink_msg_named_value_float_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - const char *name,float value) -{ - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - - put_float_by_index(msg, 0, value); // Floating point value - put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable - - mavlink_finalize_message_chan_send(msg, chan, 14, 248); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a named_value_float struct into a message * @@ -113,7 +90,12 @@ 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, const char *name, float value) { MAVLINK_ALIGNED_MESSAGE(msg, 14); - mavlink_msg_named_value_float_pack_chan_send(chan, msg, name, value); + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + + put_float_by_index(msg, 0, value); // Floating point value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable + + mavlink_finalize_message_chan_send(msg, chan, 14, 248); } #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 41210e843..41b5bc051 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 63); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a named_value_int message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param name Name of the debug variable - * @param value Signed integer value - */ -static inline void mavlink_msg_named_value_int_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - const char *name,int32_t value) -{ - msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - - put_int32_t_by_index(msg, 0, value); // Signed integer value - put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable - - mavlink_finalize_message_chan_send(msg, chan, 14, 63); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a named_value_int struct into a message * @@ -113,7 +90,12 @@ 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, const char *name, int32_t value) { MAVLINK_ALIGNED_MESSAGE(msg, 14); - mavlink_msg_named_value_int_pack_chan_send(chan, msg, name, value); + msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; + + put_int32_t_by_index(msg, 0, value); // Signed integer value + put_char_array_by_index(msg, 4, name, 10); // Name of the debug variable + + mavlink_finalize_message_chan_send(msg, chan, 14, 63); } #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 659f1a162..6f8db19f0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -101,41 +101,6 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 183); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a nav_controller_output message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param nav_roll Current desired roll in degrees - * @param nav_pitch Current desired pitch in degrees - * @param nav_bearing Current desired heading in degrees - * @param target_bearing Bearing to current waypoint/target in degrees - * @param wp_dist Distance to active waypoint in meters - * @param alt_error Current altitude error in meters - * @param aspd_error Current airspeed error in meters/second - * @param xtrack_error Current crosstrack error on x-y plane in meters - */ -static inline void mavlink_msg_nav_controller_output_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float nav_roll,float nav_pitch,int16_t nav_bearing,int16_t target_bearing,uint16_t wp_dist,float alt_error,float aspd_error,float xtrack_error) -{ - msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 26, 183); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a nav_controller_output struct into a message * @@ -167,7 +132,18 @@ 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); - mavlink_msg_nav_controller_output_pack_chan_send(chan, msg, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error); + msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 26, 183); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h index c0db135d6..d19d213ad 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 146); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a optical_flow message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param time Timestamp (UNIX) - * @param sensor_id Sensor ID - * @param flow_x Flow in pixels in x-sensor direction - * @param flow_y Flow in pixels in y-sensor direction - * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality - * @param ground_distance Ground distance in meters - */ -static inline void mavlink_msg_optical_flow_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t time,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,uint8_t quality,float ground_distance) -{ - msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; - - put_uint64_t_by_index(msg, 0, time); // 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, 146); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a optical_flow struct into a message * @@ -149,7 +118,16 @@ 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, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance) { MAVLINK_ALIGNED_MESSAGE(msg, 18); - mavlink_msg_optical_flow_pack_chan_send(chan, msg, time, sensor_id, flow_x, flow_y, quality, ground_distance); + msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW; + + put_uint64_t_by_index(msg, 0, time); // 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, 146); } #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 ddd66e781..dd55dc90d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 159); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a param_request_list message on a channel and send - * @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 - */ -static inline void mavlink_msg_param_request_list_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ - msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 2, 159); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a param_request_list struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_param_request_list_pack_chan_send(chan, msg, target_system, target_component); + msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 2, 159); } #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 c53fe9025..5b0e39f8b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 214); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a param_request_read message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id - * @param param_index Parameter index. Send -1 to use the param ID field as identifier - */ -static inline void mavlink_msg_param_request_read_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 20, 214); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a param_request_read struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_param_request_read_pack_chan_send(chan, msg, target_system, target_component, param_id, 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 + + mavlink_finalize_message_chan_send(msg, chan, 20, 214); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index 5663829e0..f54d1f07e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23, 168); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a param_set message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param param_id Onboard parameter id - * @param param_value Onboard parameter value - * @param 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 - */ -static inline void mavlink_msg_param_set_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 23, 168); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a param_set struct into a message * @@ -140,7 +111,15 @@ 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); - mavlink_msg_param_set_pack_chan_send(chan, msg, target_system, target_component, param_id, param_value, 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 + + mavlink_finalize_message_chan_send(msg, chan, 23, 168); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index dc7092add..d08ee44c1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 220); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a param_value message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param param_id Onboard parameter id - * @param param_value Onboard parameter value - * @param param_type Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t - * @param param_count Total number of onboard parameters - * @param param_index Index of this onboard parameter - */ -static inline void mavlink_msg_param_value_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 25, 220); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a param_value struct into a message * @@ -140,7 +111,15 @@ 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); - mavlink_msg_param_value_pack_chan_send(chan, msg, param_id, param_value, param_type, param_count, 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 + + mavlink_finalize_message_chan_send(msg, chan, 25, 220); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index 3a58f71a8..857e6113c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 14, 105); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a ping message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param seq PING sequence - * @param target_system 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - * @param time Unix timestamp in microseconds - */ -static inline void mavlink_msg_ping_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint32_t seq,uint8_t target_system,uint8_t target_component,uint64_t time) -{ - msg->msgid = MAVLINK_MSG_ID_PING; - - put_uint64_t_by_index(msg, 0, time); // 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 - - mavlink_finalize_message_chan_send(msg, chan, 14, 105); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a ping struct into a message * @@ -131,7 +104,14 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { MAVLINK_ALIGNED_MESSAGE(msg, 14); - mavlink_msg_ping_pack_chan_send(chan, msg, seq, target_system, target_component, time); + msg->msgid = MAVLINK_MSG_ID_PING; + + put_uint64_t_by_index(msg, 0, time); // 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 + + mavlink_finalize_message_chan_send(msg, chan, 14, 105); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index dd4af078c..66a5de368 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 126); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a position_target message on a channel and send - * @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 - */ -static inline void mavlink_msg_position_target_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float x,float y,float z,float yaw) -{ - msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 16, 126); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a position_target struct into a message * @@ -131,7 +104,14 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { MAVLINK_ALIGNED_MESSAGE(msg, 16); - mavlink_msg_position_target_pack_chan_send(chan, msg, x, y, z, yaw); + msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 16, 126); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index dea5468b9..6b15aba32 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -113,45 +113,6 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 179); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a raw_imu message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param xacc X acceleration (raw) - * @param yacc Y acceleration (raw) - * @param zacc Z acceleration (raw) - * @param xgyro Angular speed around X axis (raw) - * @param ygyro Angular speed around Y axis (raw) - * @param zgyro Angular speed around Z axis (raw) - * @param xmag X Magnetic field (raw) - * @param ymag Y Magnetic field (raw) - * @param zmag Z Magnetic field (raw) - */ -static inline void mavlink_msg_raw_imu_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) -{ - msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - - put_uint64_t_by_index(msg, 0, 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, 179); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a raw_imu struct into a message * @@ -185,7 +146,20 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { MAVLINK_ALIGNED_MESSAGE(msg, 26); - mavlink_msg_raw_imu_pack_chan_send(chan, msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); + msg->msgid = MAVLINK_MSG_ID_RAW_IMU; + + put_uint64_t_by_index(msg, 0, 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, 179); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 24d0e0547..6ea43e0d4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 136); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a raw_pressure message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param press_abs Absolute pressure (raw) - * @param press_diff1 Differential pressure 1 (raw) - * @param press_diff2 Differential pressure 2 (raw) - * @param temperature Raw Temperature measurement (raw) - */ -static inline void mavlink_msg_raw_pressure_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t 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, 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, 136); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a raw_pressure struct into a message * @@ -140,7 +111,15 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { MAVLINK_ALIGNED_MESSAGE(msg, 16); - mavlink_msg_raw_pressure_pack_chan_send(chan, msg, usec, press_abs, press_diff1, press_diff2, temperature); + msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; + + put_uint64_t_by_index(msg, 0, 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, 136); } #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 f01196248..9b99342c1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -113,45 +113,6 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 124); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a rc_channels_override message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds - */ -static inline void mavlink_msg_rc_channels_override_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a rc_channels_override struct into a message * @@ -185,7 +146,20 @@ 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); - mavlink_msg_rc_channels_override_pack_chan_send(chan, msg, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, 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 + + mavlink_finalize_message_chan_send(msg, chan, 18, 124); } #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 8e26fbcfc..c31489026 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -107,43 +107,6 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17, 252); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a rc_channels_raw message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param chan1_raw RC channel 1 value, in microseconds - * @param chan2_raw RC channel 2 value, in microseconds - * @param chan3_raw RC channel 3 value, in microseconds - * @param chan4_raw RC channel 4 value, in microseconds - * @param chan5_raw RC channel 5 value, in microseconds - * @param chan6_raw RC channel 6 value, in microseconds - * @param chan7_raw RC channel 7 value, in microseconds - * @param chan8_raw RC channel 8 value, in microseconds - * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% - */ -static inline void mavlink_msg_rc_channels_raw_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint8_t rssi) -{ - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - - 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, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% - - mavlink_finalize_message_chan_send(msg, chan, 17, 252); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a rc_channels_raw struct into a message * @@ -176,7 +139,19 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) { MAVLINK_ALIGNED_MESSAGE(msg, 17); - mavlink_msg_rc_channels_raw_pack_chan_send(chan, msg, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi); + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; + + 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, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + mavlink_finalize_message_chan_send(msg, chan, 17, 252); } #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 44845ded2..65188b6a3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -107,43 +107,6 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 17, 162); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a rc_channels_scaled message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param chan1_scaled RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan2_scaled RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan3_scaled RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan4_scaled RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan5_scaled RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan6_scaled RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan7_scaled RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% - */ -static inline void mavlink_msg_rc_channels_scaled_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - int16_t chan1_scaled,int16_t chan2_scaled,int16_t chan3_scaled,int16_t chan4_scaled,int16_t chan5_scaled,int16_t chan6_scaled,int16_t chan7_scaled,int16_t chan8_scaled,uint8_t rssi) -{ - msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - - put_int16_t_by_index(msg, 0, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 2, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 4, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 6, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 8, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 10, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 12, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_int16_t_by_index(msg, 14, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% - - mavlink_finalize_message_chan_send(msg, chan, 17, 162); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a rc_channels_scaled struct into a message * @@ -176,7 +139,19 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { MAVLINK_ALIGNED_MESSAGE(msg, 17); - mavlink_msg_rc_channels_scaled_pack_chan_send(chan, msg, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi); + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; + + put_int16_t_by_index(msg, 0, chan1_scaled); // RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 2, chan2_scaled); // RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 4, chan3_scaled); // RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 6, chan4_scaled); // RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 8, chan5_scaled); // RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 10, chan6_scaled); // RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 12, chan7_scaled); // RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_int16_t_by_index(msg, 14, chan8_scaled); // RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + put_uint8_t_by_index(msg, 16, rssi); // Receive signal strength indicator, 0: 0%, 255: 100% + + mavlink_finalize_message_chan_send(msg, chan, 17, 162); } #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 44d51935d..782f9ad04 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 148); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a request_data_stream message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system The target requested to send the message stream. - * @param target_component The target requested to send the message stream. - * @param req_stream_id The ID of the requested data stream - * @param req_message_rate The requested interval between two messages of this type - * @param start_stop 1 to start sending, 0 to stop sending. - */ -static inline void mavlink_msg_request_data_stream_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t req_stream_id,uint16_t req_message_rate,uint8_t start_stop) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a request_data_stream struct into a message * @@ -140,7 +111,15 @@ 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); - mavlink_msg_request_data_stream_pack_chan_send(chan, msg, target_system, target_component, req_stream_id, req_message_rate, 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. + + mavlink_finalize_message_chan_send(msg, chan, 6, 148); } #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 e245845f6..a2d774e26 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 @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_cha return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 148); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param time_ms Timestamp in milliseconds since system boot - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint32_t time_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_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, 148); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message * @@ -140,7 +111,15 @@ 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_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) { MAVLINK_ALIGNED_MESSAGE(msg, 20); - mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan_send(chan, msg, time_ms, roll_speed, pitch_speed, yaw_speed, thrust); + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + + put_uint32_t_by_index(msg, 0, time_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, 148); } #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 ab4fb9e38..a4c10256f 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 @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 141); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a roll_pitch_yaw_thrust_setpoint message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param time_ms Timestamp in milliseconds since system boot - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint32_t time_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_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, 141); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message * @@ -140,7 +111,15 @@ 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_ms, float roll, float pitch, float yaw, float thrust) { MAVLINK_ALIGNED_MESSAGE(msg, 20); - mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan_send(chan, msg, time_ms, roll, pitch, yaw, thrust); + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + + put_uint32_t_by_index(msg, 0, time_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, 141); } #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 7d46c97b1..9795f1975 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 25, 3); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a safety_allowed_area message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - */ -static inline void mavlink_msg_safety_allowed_area_pack_chan_send(mavlink_channel_t chan, - 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; - - 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. - - mavlink_finalize_message_chan_send(msg, chan, 25, 3); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a safety_allowed_area struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_safety_allowed_area_pack_chan_send(chan, msg, frame, p1x, p1y, p1z, p2x, p2y, p2z); + msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; + + 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. + + mavlink_finalize_message_chan_send(msg, chan, 25, 3); } #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 ec187eb40..b4d0561b2 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 @@ -107,43 +107,6 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 27, 15); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a safety_set_allowed_area message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param frame Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - * @param p1x x position 1 / Latitude 1 - * @param p1y y position 1 / Longitude 1 - * @param p1z z position 1 / Altitude 1 - * @param p2x x position 2 / Latitude 2 - * @param p2y y position 2 / Longitude 2 - * @param p2z z position 2 / Altitude 2 - */ -static inline void mavlink_msg_safety_set_allowed_area_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t frame,float p1x,float p1y,float p1z,float p2x,float p2y,float p2z) -{ - msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a safety_set_allowed_area struct into a message * @@ -176,7 +139,19 @@ 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); - mavlink_msg_safety_set_allowed_area_pack_chan_send(chan, msg, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z); + msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; + + 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); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 9c57bdea3..fc4e83d31 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -113,45 +113,6 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 222); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a scaled_imu message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param xacc X acceleration (mg) - * @param yacc Y acceleration (mg) - * @param zacc Z acceleration (mg) - * @param xgyro Angular speed around X axis (millirad /sec) - * @param ygyro Angular speed around Y axis (millirad /sec) - * @param zgyro Angular speed around Z axis (millirad /sec) - * @param xmag X Magnetic field (milli tesla) - * @param ymag Y Magnetic field (milli tesla) - * @param zmag Z Magnetic field (milli tesla) - */ -static inline void mavlink_msg_scaled_imu_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) -{ - msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_int16_t_by_index(msg, 8, xacc); // X acceleration (mg) - put_int16_t_by_index(msg, 10, yacc); // Y acceleration (mg) - put_int16_t_by_index(msg, 12, zacc); // Z acceleration (mg) - put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (millirad /sec) - put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (millirad /sec) - put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (millirad /sec) - put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (milli tesla) - put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (milli tesla) - put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (milli tesla) - - mavlink_finalize_message_chan_send(msg, chan, 26, 222); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a scaled_imu struct into a message * @@ -185,7 +146,20 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { MAVLINK_ALIGNED_MESSAGE(msg, 26); - mavlink_msg_scaled_imu_pack_chan_send(chan, msg, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag); + msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_int16_t_by_index(msg, 8, xacc); // X acceleration (mg) + put_int16_t_by_index(msg, 10, yacc); // Y acceleration (mg) + put_int16_t_by_index(msg, 12, zacc); // Z acceleration (mg) + put_int16_t_by_index(msg, 14, xgyro); // Angular speed around X axis (millirad /sec) + put_int16_t_by_index(msg, 16, ygyro); // Angular speed around Y axis (millirad /sec) + put_int16_t_by_index(msg, 18, zgyro); // Angular speed around Z axis (millirad /sec) + put_int16_t_by_index(msg, 20, xmag); // X Magnetic field (milli tesla) + put_int16_t_by_index(msg, 22, ymag); // Y Magnetic field (milli tesla) + put_int16_t_by_index(msg, 24, zmag); // Z Magnetic field (milli tesla) + + mavlink_finalize_message_chan_send(msg, chan, 26, 222); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index ead1fd040..0c9135644 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 229); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a scaled_pressure message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) - * @param press_abs Absolute pressure (hectopascal) - * @param press_diff Differential pressure 1 (hectopascal) - * @param temperature Temperature measurement (0.01 degrees celsius) - */ -static inline void mavlink_msg_scaled_pressure_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t usec,float press_abs,float press_diff,int16_t temperature) -{ - msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - - put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) - put_float_by_index(msg, 8, press_abs); // Absolute pressure (hectopascal) - put_float_by_index(msg, 12, press_diff); // Differential pressure 1 (hectopascal) - put_int16_t_by_index(msg, 16, temperature); // Temperature measurement (0.01 degrees celsius) - - mavlink_finalize_message_chan_send(msg, chan, 18, 229); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a scaled_pressure struct into a message * @@ -131,7 +104,14 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { MAVLINK_ALIGNED_MESSAGE(msg, 18); - mavlink_msg_scaled_pressure_pack_chan_send(chan, msg, usec, press_abs, press_diff, temperature); + msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; + + put_uint64_t_by_index(msg, 0, usec); // Timestamp (microseconds since UNIX epoch or microseconds since system boot) + put_float_by_index(msg, 8, press_abs); // Absolute pressure (hectopascal) + put_float_by_index(msg, 12, press_diff); // Differential pressure 1 (hectopascal) + put_int16_t_by_index(msg, 16, temperature); // Temperature measurement (0.01 degrees celsius) + + mavlink_finalize_message_chan_send(msg, chan, 18, 229); } #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 583b08bd8..2b2eb3a32 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -101,41 +101,6 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 215); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a servo_output_raw message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param servo1_raw Servo output 1 value, in microseconds - * @param servo2_raw Servo output 2 value, in microseconds - * @param servo3_raw Servo output 3 value, in microseconds - * @param servo4_raw Servo output 4 value, in microseconds - * @param servo5_raw Servo output 5 value, in microseconds - * @param servo6_raw Servo output 6 value, in microseconds - * @param servo7_raw Servo output 7 value, in microseconds - * @param servo8_raw Servo output 8 value, in microseconds - */ -static inline void mavlink_msg_servo_output_raw_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) -{ - msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - - put_uint16_t_by_index(msg, 0, servo1_raw); // Servo output 1 value, in microseconds - put_uint16_t_by_index(msg, 2, servo2_raw); // Servo output 2 value, in microseconds - put_uint16_t_by_index(msg, 4, servo3_raw); // Servo output 3 value, in microseconds - put_uint16_t_by_index(msg, 6, servo4_raw); // Servo output 4 value, in microseconds - put_uint16_t_by_index(msg, 8, servo5_raw); // Servo output 5 value, in microseconds - put_uint16_t_by_index(msg, 10, servo6_raw); // Servo output 6 value, in microseconds - put_uint16_t_by_index(msg, 12, servo7_raw); // Servo output 7 value, in microseconds - put_uint16_t_by_index(msg, 14, servo8_raw); // Servo output 8 value, in microseconds - - mavlink_finalize_message_chan_send(msg, chan, 16, 215); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a servo_output_raw struct into a message * @@ -167,7 +132,18 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { MAVLINK_ALIGNED_MESSAGE(msg, 16); - mavlink_msg_servo_output_raw_pack_chan_send(chan, msg, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw); + msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + + put_uint16_t_by_index(msg, 0, servo1_raw); // Servo output 1 value, in microseconds + put_uint16_t_by_index(msg, 2, servo2_raw); // Servo output 2 value, in microseconds + put_uint16_t_by_index(msg, 4, servo3_raw); // Servo output 3 value, in microseconds + put_uint16_t_by_index(msg, 6, servo4_raw); // Servo output 4 value, in microseconds + put_uint16_t_by_index(msg, 8, servo5_raw); // Servo output 5 value, in microseconds + put_uint16_t_by_index(msg, 10, servo6_raw); // Servo output 6 value, in microseconds + put_uint16_t_by_index(msg, 12, servo7_raw); // Servo output 7 value, in microseconds + put_uint16_t_by_index(msg, 14, servo8_raw); // Servo output 8 value, in microseconds + + mavlink_finalize_message_chan_send(msg, chan, 16, 215); } #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 index d7d6f2505..6fa3cb43f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_flight_mode.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_set_flight_mode_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 194); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_flight_mode message on a channel and send - * @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 - */ -static inline void mavlink_msg_set_flight_mode_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t flight_mode) -{ - msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; - - put_uint8_t_by_index(msg, 0, target); // The system setting the mode - put_uint8_t_by_index(msg, 1, flight_mode); // The new navigation mode - - mavlink_finalize_message_chan_send(msg, chan, 2, 194); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_flight_mode struct into a message * @@ -113,7 +90,12 @@ static inline uint16_t mavlink_msg_set_flight_mode_encode(uint8_t system_id, uin static inline void mavlink_msg_set_flight_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t flight_mode) { MAVLINK_ALIGNED_MESSAGE(msg, 2); - mavlink_msg_set_flight_mode_pack_chan_send(chan, msg, target, flight_mode); + msg->msgid = MAVLINK_MSG_ID_SET_FLIGHT_MODE; + + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, flight_mode); // The new navigation mode + + mavlink_finalize_message_chan_send(msg, chan, 2, 194); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index 9f80f4634..5d025f231 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 186); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_mode message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system setting the mode - * @param mode The new mode - */ -static inline void mavlink_msg_set_mode_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t mode) -{ - msg->msgid = MAVLINK_MSG_ID_SET_MODE; - - put_uint8_t_by_index(msg, 0, target); // The system setting the mode - put_uint8_t_by_index(msg, 1, mode); // The new mode - - mavlink_finalize_message_chan_send(msg, chan, 2, 186); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_mode struct into a message * @@ -113,7 +90,12 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { MAVLINK_ALIGNED_MESSAGE(msg, 2); - mavlink_msg_set_mode_pack_chan_send(chan, msg, target, mode); + msg->msgid = MAVLINK_MSG_ID_SET_MODE; + + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, mode); // The new mode + + mavlink_finalize_message_chan_send(msg, chan, 2, 186); } #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 ad07f2bac..c8024d0d2 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 @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 24); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_roll_pitch_yaw_speed_thrust message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param roll_speed Desired roll angular speed in rad/s - * @param pitch_speed Desired pitch angular speed in rad/s - * @param yaw_speed Desired yaw angular speed in rad/s - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,float roll_speed,float pitch_speed,float yaw_speed,float thrust) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan_send(chan, msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed, 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 + + mavlink_finalize_message_chan_send(msg, chan, 18, 24); } #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 22a57adde..f5e4af468 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 @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t s return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 100); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_roll_pitch_yaw_thrust message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param roll Desired roll angle in radians - * @param pitch Desired pitch angle in radians - * @param yaw Desired yaw angle in radians - * @param thrust Collective thrust, normalized to 0 .. 1 - */ -static inline void mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 18, 100); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_roll_pitch_yaw_thrust struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan_send(chan, msg, target_system, target_component, roll, pitch, yaw, 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 + + mavlink_finalize_message_chan_send(msg, chan, 18, 100); } #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 index fb54cc129..18b268964 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_safety_mode.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_set_safety_mode_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 8); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_safety_mode message on a channel and send - * @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. - */ -static inline void mavlink_msg_set_safety_mode_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t safety_mode) -{ - msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; - - put_uint8_t_by_index(msg, 0, target); // The system setting the mode - put_uint8_t_by_index(msg, 1, safety_mode); // The new safety mode. The MAV will reject some mode changes during flight. - - mavlink_finalize_message_chan_send(msg, chan, 2, 8); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_safety_mode struct into a message * @@ -113,7 +90,12 @@ static inline uint16_t mavlink_msg_set_safety_mode_encode(uint8_t system_id, uin static inline void mavlink_msg_set_safety_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t safety_mode) { MAVLINK_ALIGNED_MESSAGE(msg, 2); - mavlink_msg_set_safety_mode_pack_chan_send(chan, msg, target, safety_mode); + msg->msgid = MAVLINK_MSG_ID_SET_SAFETY_MODE; + + put_uint8_t_by_index(msg, 0, target); // The system setting the mode + put_uint8_t_by_index(msg, 1, safety_mode); // The new safety mode. The MAV will reject some mode changes during flight. + + mavlink_finalize_message_chan_send(msg, chan, 2, 8); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index afdcf9fd5..2390df0e6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -107,43 +107,6 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 130); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a state_correction message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param xErr x position error - * @param yErr y position error - * @param zErr z position error - * @param rollErr roll error (radians) - * @param pitchErr pitch error (radians) - * @param yawErr yaw error (radians) - * @param vxErr x velocity - * @param vyErr y velocity - * @param vzErr z velocity - */ -static inline void mavlink_msg_state_correction_pack_chan_send(mavlink_channel_t chan, - 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; - - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a state_correction struct into a message * @@ -176,7 +139,19 @@ 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); - mavlink_msg_state_correction_pack_chan_send(chan, msg, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr); + msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; + + 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); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index 23b3c0d34..8159739fd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 51, 83); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a statustext message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param severity Severity of status, 0 = info message, 255 = critical fault - * @param text Status text message, without null termination character - */ -static inline void mavlink_msg_statustext_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 51, 83); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a statustext struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_statustext_pack_chan_send(chan, msg, severity, 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 + + mavlink_finalize_message_chan_send(msg, chan, 51, 83); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index 74e6e02cd..73f61a0fd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -125,49 +125,6 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 23, 114); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a sys_status message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param 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 - * @param 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 - * @param 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 - * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt) - * @param current_battery Battery current, in milliamperes (1 = 1 milliampere) - * @param watt Watts consumed from this battery since startup - * @param battery_percent Remaining battery energy: (0%: 0, 100%: 255) - * @param errors_uart Dropped packets on all links (packets that were corrupted on reception on the MAV) - * @param errors_i2c Dropped packets on all links (packets that were corrupted on reception) - * @param errors_spi Dropped packets on all links (packets that were corrupted on reception) - * @param errors_can Dropped packets on all links (packets that were corrupted on reception) - */ -static inline void mavlink_msg_sys_status_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t onboard_control_sensors_present,uint16_t onboard_control_sensors_enabled,uint16_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,uint16_t current_battery,uint16_t watt,uint8_t battery_percent,uint16_t errors_uart,uint16_t errors_i2c,uint16_t errors_spi,uint16_t errors_can) -{ - msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - - put_uint16_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_uint16_t_by_index(msg, 2, 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_uint16_t_by_index(msg, 4, 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, 6, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - put_uint16_t_by_index(msg, 8, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) - put_uint16_t_by_index(msg, 10, current_battery); // Battery current, in milliamperes (1 = 1 milliampere) - put_uint16_t_by_index(msg, 12, watt); // Watts consumed from this battery since startup - put_uint16_t_by_index(msg, 14, errors_uart); // Dropped packets on all links (packets that were corrupted on reception on the MAV) - put_uint16_t_by_index(msg, 16, errors_i2c); // Dropped packets on all links (packets that were corrupted on reception) - put_uint16_t_by_index(msg, 18, errors_spi); // Dropped packets on all links (packets that were corrupted on reception) - put_uint16_t_by_index(msg, 20, errors_can); // Dropped packets on all links (packets that were corrupted on reception) - put_uint8_t_by_index(msg, 22, battery_percent); // Remaining battery energy: (0%: 0, 100%: 255) - - mavlink_finalize_message_chan_send(msg, chan, 23, 114); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a sys_status struct into a message * @@ -203,7 +160,22 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can) { MAVLINK_ALIGNED_MESSAGE(msg, 23); - mavlink_msg_sys_status_pack_chan_send(chan, msg, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, watt, battery_percent, errors_uart, errors_i2c, errors_spi, errors_can); + msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; + + put_uint16_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_uint16_t_by_index(msg, 2, 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_uint16_t_by_index(msg, 4, 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, 6, load); // Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + put_uint16_t_by_index(msg, 8, voltage_battery); // Battery voltage, in millivolts (1 = 1 millivolt) + put_uint16_t_by_index(msg, 10, current_battery); // Battery current, in milliamperes (1 = 1 milliampere) + put_uint16_t_by_index(msg, 12, watt); // Watts consumed from this battery since startup + put_uint16_t_by_index(msg, 14, errors_uart); // Dropped packets on all links (packets that were corrupted on reception on the MAV) + put_uint16_t_by_index(msg, 16, errors_i2c); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 18, errors_spi); // Dropped packets on all links (packets that were corrupted on reception) + put_uint16_t_by_index(msg, 20, errors_can); // Dropped packets on all links (packets that were corrupted on reception) + put_uint8_t_by_index(msg, 22, battery_percent); // Remaining battery energy: (0%: 0, 100%: 255) + + mavlink_finalize_message_chan_send(msg, chan, 23, 114); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index 3e00e85aa..f76268880 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 143); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a system_time message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. - * @param time_boot_ms Timestamp of the component clock since boot time in milliseconds. - */ -static inline void mavlink_msg_system_time_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t time_usec,uint32_t time_boot_ms) -{ - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - - put_uint64_t_by_index(msg, 0, time_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. - - mavlink_finalize_message_chan_send(msg, chan, 12, 143); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a system_time struct into a message * @@ -113,7 +90,12 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t time_boot_ms) { MAVLINK_ALIGNED_MESSAGE(msg, 12); - mavlink_msg_system_time_pack_chan_send(chan, msg, time_usec, time_boot_ms); + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; + + put_uint64_t_by_index(msg, 0, time_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. + + mavlink_finalize_message_chan_send(msg, chan, 12, 143); } #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 index dd4d13016..c911cd5fc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 191); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a system_time_utc message on a channel and send - * @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 - */ -static inline void mavlink_msg_system_time_utc_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint32_t utc_date,uint32_t utc_time) -{ - msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - - put_uint32_t_by_index(msg, 0, utc_date); // GPS UTC date ddmmyy - put_uint32_t_by_index(msg, 4, utc_time); // GPS UTC time hhmmss - - mavlink_finalize_message_chan_send(msg, chan, 8, 191); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a system_time_utc struct into a message * @@ -113,7 +90,12 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { MAVLINK_ALIGNED_MESSAGE(msg, 8); - mavlink_msg_system_time_utc_pack_chan_send(chan, msg, utc_date, utc_time); + msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; + + put_uint32_t_by_index(msg, 0, utc_date); // GPS UTC date ddmmyy + put_uint32_t_by_index(msg, 4, utc_time); // GPS UTC time hhmmss + + mavlink_finalize_message_chan_send(msg, chan, 8, 191); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index ce0a4f939..ff0a81742 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 20); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a vfr_hud message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param airspeed Current airspeed in m/s - * @param groundspeed Current ground speed in m/s - * @param heading Current heading in degrees, in compass units (0..360, 0=north) - * @param throttle Current throttle setting in integer percent, 0 to 100 - * @param alt Current altitude (MSL), in meters - * @param climb Current climb rate in meters/second - */ -static inline void mavlink_msg_vfr_hud_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 20, 20); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a vfr_hud struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_vfr_hud_pack_chan_send(chan, msg, airspeed, groundspeed, heading, throttle, alt, 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 + + mavlink_finalize_message_chan_send(msg, chan, 20, 20); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index b0d4933db..272fe4e55 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -137,53 +137,6 @@ static inline uint16_t mavlink_msg_waypoint_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 36, 205); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - * @param frame The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - * @param command The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp - * @param param1 PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - * @param param2 PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - * @param param3 PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - * @param param4 PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - * @param x PARAM5 / local: x position, global: latitude - * @param y PARAM6 / y position: global: longitude - * @param z PARAM7 / z position: global: altitude - */ -static inline void mavlink_msg_waypoint_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint8_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint struct into a message * @@ -221,7 +174,24 @@ 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); - mavlink_msg_waypoint_pack_chan_send(chan, msg, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, 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 + + mavlink_finalize_message_chan_send(msg, chan, 36, 205); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 8fd8ad086..254167e79 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 214); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_ack message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param type 0: OK, 1: Error - */ -static inline void mavlink_msg_waypoint_ack_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t type) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 3, 214); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_ack struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_waypoint_ack_pack_chan_send(chan, msg, target_system, target_component, type); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 3, 214); } #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 984f9fece..7b2449f59 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack_chan(uint8_t system_i return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 229); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_clear_all message on a channel and send - * @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 - */ -static inline void mavlink_msg_waypoint_clear_all_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 2, 229); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_clear_all struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_waypoint_clear_all_pack_chan_send(chan, msg, target_system, target_component); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 2, 229); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 7d652996f..25f68d671 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_waypoint_count_pack_chan(uint8_t system_id, u return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 8); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_count message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param count Number of Waypoints in the Sequence - */ -static inline void mavlink_msg_waypoint_count_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t count) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 4, 8); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_count struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_waypoint_count_pack_chan_send(chan, msg, target_system, target_component, count); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 4, 8); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 8f1ebf1e3..db6b91893 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -59,27 +59,6 @@ static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 101); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_current message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param seq Sequence - */ -static inline void mavlink_msg_waypoint_current_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - - put_uint16_t_by_index(msg, 0, seq); // Sequence - - mavlink_finalize_message_chan_send(msg, chan, 2, 101); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_current struct into a message * @@ -104,7 +83,11 @@ 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); - mavlink_msg_waypoint_current_pack_chan_send(chan, msg, seq); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; + + put_uint16_t_by_index(msg, 0, seq); // Sequence + + mavlink_finalize_message_chan_send(msg, chan, 2, 101); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 665eacde6..30c4d74da 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -59,27 +59,6 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 21); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_reached message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param seq Sequence - */ -static inline void mavlink_msg_waypoint_reached_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t seq) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - - put_uint16_t_by_index(msg, 0, seq); // Sequence - - mavlink_finalize_message_chan_send(msg, chan, 2, 21); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_reached struct into a message * @@ -104,7 +83,11 @@ 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); - mavlink_msg_waypoint_reached_pack_chan_send(chan, msg, seq); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; + + put_uint16_t_by_index(msg, 0, seq); // Sequence + + mavlink_finalize_message_chan_send(msg, chan, 2, 21); } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index b50eeccf9..1a6afa94e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_waypoint_request_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 51); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_request message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -static inline void mavlink_msg_waypoint_request_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 4, 51); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_request struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_waypoint_request_pack_chan_send(chan, msg, target_system, target_component, seq); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 4, 51); } #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 7160bd060..70e612eda 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack_chan(uint8_t syste return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 2, 213); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_request_list message on a channel and send - * @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 - */ -static inline void mavlink_msg_waypoint_request_list_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - - put_uint8_t_by_index(msg, 0, target_system); // System ID - put_uint8_t_by_index(msg, 1, target_component); // Component ID - - mavlink_finalize_message_chan_send(msg, chan, 2, 213); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_request_list struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_waypoint_request_list_pack_chan_send(chan, msg, target_system, target_component); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; + + put_uint8_t_by_index(msg, 0, target_system); // System ID + put_uint8_t_by_index(msg, 1, target_component); // Component ID + + mavlink_finalize_message_chan_send(msg, chan, 2, 213); } #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 8bd54abb3..48be628fd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack_chan(uint8_t system return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 80); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a waypoint_set_current message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param seq Sequence - */ -static inline void mavlink_msg_waypoint_set_current_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) -{ - msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 4, 80); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a waypoint_set_current struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_waypoint_set_current_pack_chan_send(chan, msg, target_system, target_component, seq); + msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 4, 80); } #endif diff --git a/thirdParty/mavlink/include/common/testsuite.h b/thirdParty/mavlink/include/common/testsuite.h index 119d6af36..b42df8642 100644 --- a/thirdParty/mavlink/include/common/testsuite.h +++ b/thirdParty/mavlink/include/common/testsuite.h @@ -51,8 +51,7 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) for (i=0; imagic, msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES); - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); + return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; } union __mavlink_bitfield { @@ -429,7 +451,7 @@ MAVLINK_HELPER void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* #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, msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES); + MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len); #else /* fallback to one byte at a time */ uint8_t *buffer = (uint8_t *)&msg->magic; diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 3244881ea..0a759b618 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -13,6 +13,10 @@ #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + #ifndef MAVLINK_CRC_EXTRA #define MAVLINK_CRC_EXTRA 1 #endif diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index dfee24700..4d7053dab 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -99,40 +99,6 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 11); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a heartbeat message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - * @param autopilot Autopilot type / class. defined in MAV_CLASS ENUM - * @param mode System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - * @param nav_mode Navigation mode, see MAV_NAV_MODE ENUM - * @param status System status flag, see MAV_STATUS ENUM - * @param safety_status System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation - * @param 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 - */ -static inline void mavlink_msg_heartbeat_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 8, 11); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a heartbeat struct into a message * @@ -163,7 +129,18 @@ 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); - mavlink_msg_heartbeat_pack_chan_send(chan, msg, type, autopilot, mode, nav_mode, status, safety_status, link_status); + msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 8, 11); } #endif diff --git a/thirdParty/mavlink/include/minimal/testsuite.h b/thirdParty/mavlink/include/minimal/testsuite.h index cbbff8d58..355177820 100644 --- a/thirdParty/mavlink/include/minimal/testsuite.h +++ b/thirdParty/mavlink/include/minimal/testsuite.h @@ -51,8 +51,7 @@ static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id) for (i=0; imsgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a attitude_control struct into a message * @@ -176,7 +139,19 @@ 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); - mavlink_msg_attitude_control_pack_chan_send(chan, msg, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual); + msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; + + 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); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index 7f444d0af..4024c88e7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -101,41 +101,6 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 53, 88); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a brief_feature message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param x x position in m - * @param y y position in m - * @param z z position in m - * @param orientation_assignment Orientation assignment 0: false, 1:true - * @param size Size in pixels - * @param orientation Orientation - * @param descriptor Descriptor - * @param response Harris operator response at this location - */ -static inline void mavlink_msg_brief_feature_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float x,float y,float z,uint8_t orientation_assignment,uint16_t size,uint16_t orientation,const uint8_t *descriptor,float response) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a brief_feature struct into a message * @@ -167,7 +132,18 @@ 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); - mavlink_msg_brief_feature_pack_chan_send(chan, msg, x, y, z, orientation_assignment, size, orientation, descriptor, 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 + + mavlink_finalize_message_chan_send(msg, chan, 53, 88); } #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 acd946ca6..4bbba77b0 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 8, 148); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a data_transmission_handshake message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - * @param size total data size in bytes (set on ACK only) - * @param packets number of packets beeing sent (set on ACK only) - * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - * @param jpg_quality JPEG quality out of [1,100] - */ -static inline void mavlink_msg_data_transmission_handshake_pack_chan_send(mavlink_channel_t chan, - 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] - - mavlink_finalize_message_chan_send(msg, chan, 8, 148); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a data_transmission_handshake struct into a message * @@ -140,7 +111,15 @@ 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); - mavlink_msg_data_transmission_handshake_pack_chan_send(chan, msg, type, size, packets, payload, 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] + + mavlink_finalize_message_chan_send(msg, chan, 8, 148); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index 86f10d6cb..efddee57d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 223); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a encapsulated_data message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param seqnr sequence number (starting with 0 on every transmission) - * @param data image data bytes - */ -static inline void mavlink_msg_encapsulated_data_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 255, 223); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a encapsulated_data struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_encapsulated_data_pack_chan_send(chan, msg, seqnr, 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 + + mavlink_finalize_message_chan_send(msg, chan, 255, 223); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index fae307c4d..a55ee69b1 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -191,12 +191,23 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 92, 224); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - /** - * @brief Pack a image_available message on a channel and send - * @param chan The MAVLink channel this message was sent over + * @brief Encode a image_available 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 image_available C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_available_t* image_available) +{ + return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); +} + +/** + * @brief Send a image_available message + * @param chan MAVLink channel to send the message + * * @param cam_id Camera id * @param cam_no Camera # (starts with 0) * @param timestamp Timestamp @@ -221,10 +232,11 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ -static inline void mavlink_msg_image_available_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t cam_id,uint8_t cam_no,uint64_t timestamp,uint64_t valid_until,uint32_t img_seq,uint32_t img_buf_index,uint16_t width,uint16_t height,uint16_t depth,uint8_t channels,uint32_t key,uint32_t exposure,float gain,float roll,float pitch,float yaw,float local_z,float lat,float lon,float alt,float ground_x,float ground_y,float ground_z) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { + MAVLINK_ALIGNED_MESSAGE(msg, 92); msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; put_uint64_t_by_index(msg, 0, cam_id); // Camera id @@ -253,57 +265,6 @@ static inline void mavlink_msg_image_available_pack_chan_send(mavlink_channel_t mavlink_finalize_message_chan_send(msg, chan, 92, 224); } -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - -/** - * @brief Encode a image_available 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 image_available C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_available_t* image_available) -{ - return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); -} - -/** - * @brief Send a image_available message - * @param chan MAVLink channel to send the message - * - * @param cam_id Camera id - * @param cam_no Camera # (starts with 0) - * @param timestamp Timestamp - * @param valid_until Until which timestamp this buffer will stay valid - * @param img_seq The image sequence number - * @param img_buf_index Position of the image in the buffer, starts with 0 - * @param width Image width - * @param height Image height - * @param depth Image depth - * @param channels Image channels - * @param key Shared memory area key - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ - MAVLINK_ALIGNED_MESSAGE(msg, 92); - mavlink_msg_image_available_pack_chan_send(chan, msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z); -} #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 48713c9e9..01c8e5379 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -59,27 +59,6 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 1, 95); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a image_trigger_control message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param enable 0 to disable, 1 to enable - */ -static inline void mavlink_msg_image_trigger_control_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t enable) -{ - msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - - put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable - - mavlink_finalize_message_chan_send(msg, chan, 1, 95); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a image_trigger_control struct into a message * @@ -104,7 +83,11 @@ 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); - mavlink_msg_image_trigger_control_pack_chan_send(chan, msg, enable); + msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; + + put_uint8_t_by_index(msg, 0, enable); // 0 to disable, 1 to enable + + mavlink_finalize_message_chan_send(msg, chan, 1, 95); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index a6c48878d..56c128d32 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -125,49 +125,6 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 52, 86); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a image_triggered message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param timestamp Timestamp - * @param seq IMU seq - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - * @param local_z Local frame Z coordinate (height over ground) - * @param lat GPS X coordinate - * @param lon GPS Y coordinate - * @param alt Global frame altitude - * @param ground_x Ground truth X - * @param ground_y Ground truth Y - * @param ground_z Ground truth Z - */ -static inline void mavlink_msg_image_triggered_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t timestamp,uint32_t seq,float roll,float pitch,float yaw,float local_z,float lat,float lon,float alt,float ground_x,float ground_y,float ground_z) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a image_triggered struct into a message * @@ -203,7 +160,22 @@ 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); - mavlink_msg_image_triggered_pack_chan_send(chan, msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, 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 + + mavlink_finalize_message_chan_send(msg, chan, 52, 86); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index 6a845909d..dac1c42b6 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 26, 249); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a marker message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param id ID - * @param x x position - * @param y y position - * @param z z position - * @param roll roll orientation - * @param pitch pitch orientation - * @param yaw yaw orientation - */ -static inline void mavlink_msg_marker_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 26, 249); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a marker struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_marker_pack_chan_send(chan, msg, id, x, y, z, roll, pitch, yaw); + msg->msgid = MAVLINK_MSG_ID_MARKER; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 26, 249); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 07d369f38..24b35b4b4 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 106, 90); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a pattern_detected message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Pattern, 1: Letter - * @param confidence Confidence of detection - * @param file Pattern file name - * @param detected Accepted as true detection, 0 no, 1 yes - */ -static inline void mavlink_msg_pattern_detected_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 106, 90); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a pattern_detected struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_pattern_detected_pack_chan_send(chan, msg, type, confidence, file, 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 + + mavlink_finalize_message_chan_send(msg, chan, 106, 90); } #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 d7a9522dd..2afaeeaa2 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -101,41 +101,6 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 43, 95); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a point_of_interest message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param x X Position - * @param y Y Position - * @param z Z Position - * @param name POI name - */ -static inline void mavlink_msg_point_of_interest_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t color,uint8_t coordinate_system,uint16_t timeout,float x,float y,float z,const char *name) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a point_of_interest struct into a message * @@ -167,7 +132,18 @@ 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); - mavlink_msg_point_of_interest_pack_chan_send(chan, msg, type, color, coordinate_system, timeout, x, y, z, 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 + + mavlink_finalize_message_chan_send(msg, chan, 43, 95); } #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 3a2ec961b..708aed1fd 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 @@ -119,47 +119,6 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 55, 36); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a point_of_interest_connection message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param type 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - * @param color 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - * @param coordinate_system 0: global, 1:local - * @param timeout 0: no timeout, >1: timeout in seconds - * @param xp1 X1 Position - * @param yp1 Y1 Position - * @param zp1 Z1 Position - * @param xp2 X2 Position - * @param yp2 Y2 Position - * @param zp2 Z2 Position - * @param name POI connection name - */ -static inline void mavlink_msg_point_of_interest_connection_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t type,uint8_t color,uint8_t coordinate_system,uint16_t timeout,float xp1,float yp1,float zp1,float xp2,float yp2,float zp2,const char *name) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a point_of_interest_connection struct into a message * @@ -194,7 +153,21 @@ 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); - mavlink_msg_point_of_interest_connection_pack_chan_send(chan, msg, type, color, coordinate_system, timeout, xp1, yp1, zp1, xp2, yp2, zp2, 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 + + mavlink_finalize_message_chan_send(msg, chan, 55, 36); } #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 ce8c045ab..4aca8c61c 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 @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack_chan(uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 244); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a position_control_offset_set message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system System ID - * @param target_component Component ID - * @param x x position offset - * @param y y position offset - * @param z z position offset - * @param yaw yaw orientation offset in radians, 0 = NORTH - */ -static inline void mavlink_msg_position_control_offset_set_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 18, 244); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a position_control_offset_set struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_position_control_offset_set_pack_chan_send(chan, msg, target_system, target_component, x, y, z, 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 + + mavlink_finalize_message_chan_send(msg, chan, 18, 244); } #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 71a304fe1..2e00c6e40 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -83,35 +83,6 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 28); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a position_control_setpoint message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - */ -static inline void mavlink_msg_position_control_setpoint_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 18, 28); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a position_control_setpoint struct into a message * @@ -140,7 +111,15 @@ 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); - mavlink_msg_position_control_setpoint_pack_chan_send(chan, msg, id, x, y, z, 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 + + mavlink_finalize_message_chan_send(msg, chan, 18, 28); } #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 8c9cd2bea..37de388db 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 @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack_chan(uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 11); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a position_control_setpoint_set message on a channel and send - * @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 id ID of waypoint, 0 for plain position - * @param x x position - * @param y y position - * @param z z position - * @param yaw yaw orientation in radians, 0 = NORTH - */ -static inline void mavlink_msg_position_control_setpoint_set_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t id,float x,float y,float z,float yaw) -{ - msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 20, 11); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a position_control_setpoint_set struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_position_control_setpoint_set_pack_chan_send(chan, msg, target_system, target_component, id, x, y, z, yaw); + msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 20, 11); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index ee7c52919..68722c5b9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 16, 182); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a raw_aux message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param adc1 ADC1 (J405 ADC3, LPC2148 AD0.6) - * @param adc2 ADC2 (J405 ADC5, LPC2148 AD0.2) - * @param adc3 ADC3 (J405 ADC6, LPC2148 AD0.1) - * @param adc4 ADC4 (J405 ADC7, LPC2148 AD1.3) - * @param vbat Battery voltage - * @param temp Temperature (degrees celcius) - * @param baro Barometric pressure (hecto Pascal) - */ -static inline void mavlink_msg_raw_aux_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t adc1,uint16_t adc2,uint16_t adc3,uint16_t adc4,uint16_t vbat,int16_t temp,int32_t baro) -{ - msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - - 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) - - mavlink_finalize_message_chan_send(msg, chan, 16, 182); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a raw_aux struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_raw_aux_pack_chan_send(chan, msg, adc1, adc2, adc3, adc4, vbat, temp, baro); + msg->msgid = MAVLINK_MSG_ID_RAW_AUX; + + 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) + + mavlink_finalize_message_chan_send(msg, chan, 16, 182); } #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 392cfec0b..f2957a38b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 11, 108); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a set_cam_shutter message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param cam_no Camera id - * @param cam_mode Camera mode: 0 = auto, 1 = manual - * @param trigger_pin Trigger pin, 0-3 for PtGrey FireFly - * @param interval Shutter interval, in microseconds - * @param exposure Exposure time, in microseconds - * @param gain Camera gain - */ -static inline void mavlink_msg_set_cam_shutter_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t cam_no,uint8_t cam_mode,uint8_t trigger_pin,uint16_t interval,uint16_t exposure,float gain) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a set_cam_shutter struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_set_cam_shutter_pack_chan_send(chan, msg, cam_no, cam_mode, trigger_pin, interval, exposure, 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 + + mavlink_finalize_message_chan_send(msg, chan, 11, 108); } #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 bbc5d7750..b3c6ee26c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 56); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a vicon_position_estimate message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -static inline void mavlink_msg_vicon_position_estimate_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 32, 56); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a vicon_position_estimate struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_vicon_position_estimate_pack_chan_send(chan, msg, usec, x, y, z, roll, pitch, yaw); + msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 32, 56); } #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 4f9b5a10f..270fc74af 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 158); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a vision_position_estimate message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X position - * @param y Global Y position - * @param z Global Z position - * @param roll Roll angle in rad - * @param pitch Pitch angle in rad - * @param yaw Yaw angle in rad - */ -static inline void mavlink_msg_vision_position_estimate_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 32, 158); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a vision_position_estimate struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_vision_position_estimate_pack_chan_send(chan, msg, usec, x, y, z, roll, pitch, yaw); + msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 32, 158); } #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 0065c116c..4897307fa 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 208); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a vision_speed_estimate message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param usec Timestamp (milliseconds) - * @param x Global X speed - * @param y Global Y speed - * @param z Global Z speed - */ -static inline void mavlink_msg_vision_speed_estimate_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint64_t usec,float x,float y,float z) -{ - 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 - - mavlink_finalize_message_chan_send(msg, chan, 20, 208); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a vision_speed_estimate struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_vision_speed_estimate_pack_chan_send(chan, msg, usec, x, y, z); + 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 + + mavlink_finalize_message_chan_send(msg, chan, 20, 208); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index d1ddbab29..a3e3f52be 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 6, 162); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a watchdog_command message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target_system_id Target system ID - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param command_id Command ID - */ -static inline void mavlink_msg_watchdog_command_pack_chan_send(mavlink_channel_t chan, - 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; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 6, 162); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a watchdog_command struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_watchdog_command_pack_chan_send(chan, msg, target_system_id, watchdog_id, process_id, command_id); + 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 + + mavlink_finalize_message_chan_send(msg, chan, 6, 162); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index a85760dcc..fd51415c9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 153); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a watchdog_heartbeat message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_count Number of processes - */ -static inline void mavlink_msg_watchdog_heartbeat_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_count) -{ - msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - - put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID - put_uint16_t_by_index(msg, 2, process_count); // Number of processes - - mavlink_finalize_message_chan_send(msg, chan, 4, 153); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a watchdog_heartbeat struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_watchdog_heartbeat_pack_chan_send(chan, msg, watchdog_id, process_count); + msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; + + put_uint16_t_by_index(msg, 0, watchdog_id); // Watchdog ID + put_uint16_t_by_index(msg, 2, process_count); // Number of processes + + mavlink_finalize_message_chan_send(msg, chan, 4, 153); } #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 2bcc5ee59..4dc458166 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -84,35 +84,6 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 255, 16); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a watchdog_process_info message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param name Process name - * @param arguments Process arguments - * @param timeout Timeout (seconds) - */ -static inline void mavlink_msg_watchdog_process_info_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 255, 16); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a watchdog_process_info struct into a message * @@ -141,7 +112,15 @@ 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); - mavlink_msg_watchdog_process_info_pack_chan_send(chan, msg, watchdog_id, process_id, name, arguments, 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 + + mavlink_finalize_message_chan_send(msg, chan, 255, 16); } #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 32bd57429..fbe32361f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 29); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a watchdog_process_status message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param watchdog_id Watchdog ID - * @param process_id Process ID - * @param state Is running / finished / suspended / crashed - * @param muted Is muted - * @param pid PID - * @param crashes Number of crashes - */ -static inline void mavlink_msg_watchdog_process_status_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint16_t watchdog_id,uint16_t process_id,uint8_t state,uint8_t muted,int32_t pid,uint16_t crashes) -{ - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a watchdog_process_status struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_watchdog_process_status_pack_chan_send(chan, msg, watchdog_id, process_id, state, muted, pid, 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 + + mavlink_finalize_message_chan_send(msg, chan, 12, 29); } #endif diff --git a/thirdParty/mavlink/include/pixhawk/testsuite.h b/thirdParty/mavlink/include/pixhawk/testsuite.h index 9c56c96df..fad934f34 100644 --- a/thirdParty/mavlink/include/pixhawk/testsuite.h +++ b/thirdParty/mavlink/include/pixhawk/testsuite.h @@ -49,8 +49,7 @@ static void mavlink_test_set_cam_shutter(uint8_t system_id, uint8_t component_id for (i=0; ilen + MAVLINK_NUM_NON_PAYLOAD_BYTES; } -typedef union { - uint8_t b[2]; - uint16_t i; -} generic_16bit; - -typedef union { - uint8_t b[4]; - uint32_t i; - float f; -} generic_32bit; - -typedef union { - uint8_t b[8]; - uint64_t i; - double d; -} generic_64bit; - -/** - * @brief Place an unsigned byte into the buffer - * - * @param b the byte to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_uint8_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint8_t b) +#if MAVLINK_NEED_BYTE_SWAP +static inline void byte_swap_2(uint8_t *dst, const uint8_t *src) { - msg->payload.u8[wire_offset] = b; + dst[0] = src[1]; + dst[1] = src[0]; } - -/** - * @brief Place a signed byte into the buffer - * - * @param b the byte to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_int8_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int8_t b) +static inline void byte_swap_4(uint8_t *dst, const uint8_t *src) { - msg->payload.i8[wire_offset] = b; + dst[0] = src[3]; + dst[1] = src[2]; + dst[2] = src[1]; + dst[3] = src[0]; } - -/** - * @brief Place a char into the buffer - * - * @param b the byte to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_char_by_index(mavlink_message_t *msg, uint8_t wire_offset, char b) +static inline void byte_swap_8(uint8_t *dst, const uint8_t *src) { - msg->payload.c[wire_offset] = b; + dst[0] = src[7]; + dst[1] = src[6]; + dst[2] = src[5]; + dst[3] = src[4]; + dst[4] = src[3]; + dst[5] = src[2]; + dst[6] = src[1]; + dst[7] = src[0]; } - -/** - * @brief Place two unsigned bytes into the buffer - * - * @param b the bytes to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_uint16_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint16_t b) +#elif !MAVLINK_ALIGNED_FIELDS +static inline void byte_copy_2(uint8_t *dst, const uint8_t *src) { -#if MAVLINK_NEED_BYTE_SWAP - generic_16bit g; - g.i = b; - msg->payload.u8[wire_offset+0] = g.b[1]; - msg->payload.u8[wire_offset+1] = g.b[0]; -#else - msg->payload.u16[wire_offset/2] = b; -#endif + dst[0] = src[0]; + dst[1] = src[1]; } - -/** - * @brief Place two signed bytes into the buffer - * - * @param b the bytes to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_int16_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int16_t b) +static inline void byte_copy_4(uint8_t *dst, const uint8_t *src) { -#if MAVLINK_NEED_BYTE_SWAP - put_uint16_t_by_index(msg, wire_offset, b); -#else - msg->payload.i16[wire_offset/2] = b; -#endif + dst[0] = src[0]; + dst[1] = src[1]; + dst[2] = src[2]; + dst[3] = src[3]; } - -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_uint32_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint32_t b) +static inline void byte_copy_8(uint8_t *dst, const uint8_t *src) { -#if MAVLINK_NEED_BYTE_SWAP - generic_32bit g; - g.i = b; - msg->payload.u8[wire_offset+0] = g.b[3]; - msg->payload.u8[wire_offset+1] = g.b[2]; - msg->payload.u8[wire_offset+2] = g.b[1]; - msg->payload.u8[wire_offset+3] = g.b[0]; -#else - msg->payload.u32[wire_offset/4] = b; -#endif + memcpy(dst, src, 8); } - -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_int32_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int32_t b) -{ -#if MAVLINK_NEED_BYTE_SWAP - put_uint32_t_by_index(msg, wire_offset, b); -#else - msg->payload.i32[wire_offset/4] = b; #endif -} -/** - * @brief Place four unsigned bytes into the buffer - * - * @param b the bytes to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_uint64_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, uint64_t b) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_64bit r; - r.i = b; - msg->payload.u8[wire_offset+0] = r.b[7]; - msg->payload.u8[wire_offset+1] = r.b[6]; - msg->payload.u8[wire_offset+2] = r.b[5]; - msg->payload.u8[wire_offset+3] = r.b[4]; - msg->payload.u8[wire_offset+4] = r.b[3]; - msg->payload.u8[wire_offset+5] = r.b[2]; - msg->payload.u8[wire_offset+6] = r.b[1]; - msg->payload.u8[wire_offset+7] = r.b[0]; -#else - msg->payload.u64[wire_offset/8] = b; -#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 -/** - * @brief Place four signed bytes into the buffer - * - * @param b the bytes to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_int64_t_by_index(mavlink_message_t *msg, uint8_t wire_offset, int64_t b) -{ #if MAVLINK_NEED_BYTE_SWAP - put_uint64_t_by_index(msg, wire_offset, b); -#else - msg->payload.i64[wire_offset/8] = b; +#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) +#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 #endif -} -/** - * @brief Place a float into the buffer - * - * @param b the float to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_float_by_index(mavlink_message_t *msg, uint8_t wire_offset, float b) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_32bit g; - g.f = b; - put_uint32_t_by_index(msg, wire_offset, g.i); -#else - msg->payload.f[wire_offset/4] = b; -#endif -} - -/** - * @brief Place a double into the buffer - * - * @param b the double to add - * @param wire_offset the position in the packet - * @param buffer the packet buffer - */ -static inline void put_double_by_index(mavlink_message_t *msg, uint8_t wire_offset, double b) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_64bit g; - g.d = b; - put_uint64_t_by_index(msg, wire_offset, g.i); -#else - msg->payload.d[wire_offset/8] = 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) { - memcpy(&msg->payload.c[wire_offset], b, array_length); + if (b == NULL) { + memset(&msg->payload.c[wire_offset], 0, array_length); + } else { + memcpy(&msg->payload.c[wire_offset], b, array_length); + } } /* @@ -276,7 +162,11 @@ static inline void put_char_array_by_index(mavlink_message_t *msg, uint8_t wire_ */ 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) { - memcpy(&msg->payload.u8[wire_offset], b, array_length); + if (b == NULL) { + memset(&msg->payload.u8[wire_offset], 0, array_length); + } else { + memcpy(&msg->payload.u8[wire_offset], b, array_length); + } } /* @@ -284,23 +174,35 @@ static inline void put_uint8_t_array_by_index(mavlink_message_t *msg, uint8_t wi */ 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) { - memcpy(&msg->payload.i8[wire_offset], b, array_length); + if (b == NULL) { + memset(&msg->payload.i8[wire_offset], 0, array_length); + } else { + memcpy(&msg->payload.i8[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) \ { \ - uint16_t i; \ - for (i=0; ipayload.u8[wire_offset], 0, array_length*sizeof(TYPE)); \ + } else { \ + uint16_t i; \ + for (i=0; ipayload.V[wire_offset/sizeof(TYPE)], b, array_length*sizeof(TYPE)); \ + if (b == NULL) { \ + memset(&msg->payload.u8[wire_offset], 0, array_length*sizeof(TYPE)); \ + } else { \ + memcpy(&msg->payload.V[wire_offset/sizeof(TYPE)], b, array_length*sizeof(TYPE)); \ + } \ } #endif @@ -317,109 +219,44 @@ PUT_ARRAY_BY_INDEX(double, d) #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] -static inline uint16_t MAVLINK_MSG_RETURN_uint16_t(const mavlink_message_t *msg, uint8_t wire_offset) -{ #if MAVLINK_NEED_BYTE_SWAP - generic_16bit r; - r.b[1] = msg->payload.u8[wire_offset+0]; - r.b[0] = msg->payload.u8[wire_offset+1]; - return r.i; -#else - return msg->payload.u16[wire_offset/2]; -#endif -} - -static inline int16_t MAVLINK_MSG_RETURN_int16_t(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - return (int16_t)MAVLINK_MSG_RETURN_uint16_t(msg, wire_offset); -#else - return msg->payload.i16[wire_offset/2]; -#endif -} - -static inline uint32_t MAVLINK_MSG_RETURN_uint32_t(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_32bit r; - r.b[3] = msg->payload.u8[wire_offset+0]; - r.b[2] = msg->payload.u8[wire_offset+1]; - r.b[1] = msg->payload.u8[wire_offset+2]; - r.b[0] = msg->payload.u8[wire_offset+3]; - return r.i; -#else - return msg->payload.u32[wire_offset/4]; -#endif -} - -static inline int32_t MAVLINK_MSG_RETURN_int32_t(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - return (int32_t)MAVLINK_MSG_RETURN_uint32_t(msg, wire_offset); -#else - return msg->payload.i32[wire_offset/4]; -#endif -} - -static inline uint64_t MAVLINK_MSG_RETURN_uint64_t(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_64bit r; - r.b[7] = msg->payload.u8[wire_offset+0]; - r.b[6] = msg->payload.u8[wire_offset+1]; - r.b[5] = msg->payload.u8[wire_offset+2]; - r.b[4] = msg->payload.u8[wire_offset+3]; - r.b[3] = msg->payload.u8[wire_offset+4]; - r.b[2] = msg->payload.u8[wire_offset+5]; - r.b[1] = msg->payload.u8[wire_offset+6]; - r.b[0] = msg->payload.u8[wire_offset+7]; - return r.i; -#else - return msg->payload.u64[wire_offset/8]; -#endif -} - -static inline int64_t MAVLINK_MSG_RETURN_int64_t(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - return (int64_t)MAVLINK_MSG_RETURN_uint64_t(msg, wire_offset); -#else - return msg->payload.i64[wire_offset/8]; -#endif -} - -static inline float MAVLINK_MSG_RETURN_float(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_32bit r; - r.b[3] = msg->payload.u8[wire_offset+0]; - r.b[2] = msg->payload.u8[wire_offset+1]; - r.b[1] = msg->payload.u8[wire_offset+2]; - r.b[0] = msg->payload.u8[wire_offset+3]; - return r.f; -#else - return msg->payload.f[wire_offset/4]; -#endif -} - -static inline double MAVLINK_MSG_RETURN_double(const mavlink_message_t *msg, uint8_t wire_offset) -{ -#if MAVLINK_NEED_BYTE_SWAP - generic_64bit r; - r.b[7] = msg->payload.u8[wire_offset+0]; - r.b[6] = msg->payload.u8[wire_offset+1]; - r.b[5] = msg->payload.u8[wire_offset+2]; - r.b[4] = msg->payload.u8[wire_offset+3]; - r.b[3] = msg->payload.u8[wire_offset+4]; - r.b[2] = msg->payload.u8[wire_offset+5]; - r.b[1] = msg->payload.u8[wire_offset+6]; - r.b[0] = msg->payload.u8[wire_offset+7]; - return r.d; -#else - return msg->payload.d[wire_offset/8]; -#endif -} - +#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; } + +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) + +#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; } + +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) + +#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] +#endif // MAVLINK_NEED_BYTE_SWAP static inline uint16_t MAVLINK_MSG_RETURN_char_array(const mavlink_message_t *msg, char *value, uint8_t array_length, uint8_t wire_offset) @@ -459,7 +296,7 @@ static inline uint16_t MAVLINK_MSG_RETURN_## TYPE ##_array(const mavlink_message uint8_t array_length, uint8_t wire_offset) \ { \ memcpy(value, &msg->payload.V[wire_offset/sizeof(TYPE)], array_length*sizeof(TYPE)); \ - return array_length*sizeof(value[0]); \ + return array_length*sizeof(TYPE); \ } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index 8eb3f2357..d28bbc7b9 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -13,6 +13,10 @@ #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN #endif +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + #ifndef MAVLINK_CRC_EXTRA #define MAVLINK_CRC_EXTRA 1 #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 7cea7c28f..fc8714581 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 10, 232); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a air_data message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param dynamicPressure Dynamic pressure (Pa) - * @param staticPressure Static pressure (Pa) - * @param temperature Board temperature - */ -static inline void mavlink_msg_air_data_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float dynamicPressure,float staticPressure,uint16_t temperature) -{ - msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 10, 232); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a air_data struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_air_data_pack_chan_send(chan, msg, dynamicPressure, staticPressure, temperature); + msg->msgid = MAVLINK_MSG_ID_AIR_DATA; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 10, 232); } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index 2eb43c1a5..e14f1e824 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 75); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a cpu_load message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param sensLoad Sensor DSC Load - * @param ctrlLoad Control DSC Load - * @param batVolt Battery Voltage in millivolts - */ -static inline void mavlink_msg_cpu_load_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t sensLoad,uint8_t ctrlLoad,uint16_t batVolt) -{ - msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 4, 75); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a cpu_load struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_cpu_load_pack_chan_send(chan, msg, sensLoad, ctrlLoad, batVolt); + msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 4, 75); } #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 551ca57e2..ae4fc86fb 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -65,29 +65,6 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 104); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a ctrl_srfc_pt message on a channel and send - * @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 commands - * @param bitfieldPt Bitfield containing the PT configuration - */ -static inline void mavlink_msg_ctrl_srfc_pt_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,uint16_t bitfieldPt) -{ - msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 3, 104); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a ctrl_srfc_pt struct into a message * @@ -113,7 +90,12 @@ 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); - mavlink_msg_ctrl_srfc_pt_pack_chan_send(chan, msg, target, bitfieldPt); + msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 3, 104); } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index be2bef8e3..cf526b262 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 167); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a data_log message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param fl_1 Log value 1 - * @param fl_2 Log value 2 - * @param fl_3 Log value 3 - * @param fl_4 Log value 4 - * @param fl_5 Log value 5 - * @param fl_6 Log value 6 - */ -static inline void mavlink_msg_data_log_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 24, 167); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a data_log struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_data_log_pack_chan_send(chan, msg, fl_1, fl_2, fl_3, fl_4, fl_5, 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 + + mavlink_finalize_message_chan_send(msg, chan, 24, 167); } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index 656703586..05002adec 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 18, 2); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a diagnostic message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param diagFl1 Diagnostic float 1 - * @param diagFl2 Diagnostic float 2 - * @param diagFl3 Diagnostic float 3 - * @param diagSh1 Diagnostic short 1 - * @param diagSh2 Diagnostic short 2 - * @param diagSh3 Diagnostic short 3 - */ -static inline void mavlink_msg_diagnostic_pack_chan_send(mavlink_channel_t chan, - 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 - - mavlink_finalize_message_chan_send(msg, chan, 18, 2); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a diagnostic struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_diagnostic_pack_chan_send(chan, msg, diagFl1, diagFl2, diagFl3, diagSh1, diagSh2, 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 + + mavlink_finalize_message_chan_send(msg, chan, 18, 2); } #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 cef5fb0f8..7ef93956f 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -95,39 +95,6 @@ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, ui return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 7, 16); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a gps_date_time message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param year Year reported by Gps - * @param month Month reported by Gps - * @param day Day reported by Gps - * @param hour Hour reported by Gps - * @param min Min reported by Gps - * @param sec Sec reported by Gps - * @param visSat Visible sattelites reported by Gps - */ -static inline void mavlink_msg_gps_date_time_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t year,uint8_t month,uint8_t day,uint8_t hour,uint8_t min,uint8_t sec,uint8_t visSat) -{ - msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 7, 16); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a gps_date_time struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_gps_date_time_pack_chan_send(chan, msg, year, month, day, hour, min, sec, visSat); + msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 7, 16); } #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 46921f5b4..32d94ff58 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -77,33 +77,6 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 13, 146); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a mid_lvl_cmds message on a channel and send - * @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 commands - * @param hCommand Commanded Airspeed - * @param uCommand Log value 2 - * @param rCommand Log value 3 - */ -static inline void mavlink_msg_mid_lvl_cmds_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,float hCommand,float uCommand,float rCommand) -{ - 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 - - mavlink_finalize_message_chan_send(msg, chan, 13, 146); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a mid_lvl_cmds struct into a message * @@ -131,7 +104,14 @@ 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); - mavlink_msg_mid_lvl_cmds_pack_chan_send(chan, msg, target, hCommand, uCommand, rCommand); + 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 + + mavlink_finalize_message_chan_send(msg, chan, 13, 146); } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 8ce2800de..fd5a367ac 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -89,37 +89,6 @@ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 168); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a sensor_bias message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param axBias Accelerometer X bias (m/s) - * @param ayBias Accelerometer Y bias (m/s) - * @param azBias Accelerometer Z bias (m/s) - * @param gxBias Gyro X bias (rad/s) - * @param gyBias Gyro Y bias (rad/s) - * @param gzBias Gyro Z bias (rad/s) - */ -static inline void mavlink_msg_sensor_bias_pack_chan_send(mavlink_channel_t chan, - 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) - - mavlink_finalize_message_chan_send(msg, chan, 24, 168); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a sensor_bias struct into a message * @@ -149,7 +118,16 @@ 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); - mavlink_msg_sensor_bias_pack_chan_send(chan, msg, axBias, ayBias, azBias, gxBias, gyBias, 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) + + mavlink_finalize_message_chan_send(msg, chan, 24, 168); } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index 134675de0..765d6332d 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uin return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 65); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a slugs_action message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param target The system reporting the action - * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - * @param actionVal Value associated with the action - */ -static inline void mavlink_msg_slugs_action_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t target,uint8_t actionId,uint16_t actionVal) -{ - msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 4, 65); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a slugs_action struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_slugs_action_pack_chan_send(chan, msg, target, actionId, actionVal); + msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 4, 65); } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index 2ce93dd19..2e20f14a4 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -107,43 +107,6 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 30, 120); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a slugs_navigation message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param u_m Measured Airspeed prior to the Nav Filter - * @param phi_c Commanded Roll - * @param theta_c Commanded Pitch - * @param psiDot_c Commanded Turn rate - * @param ay_body Y component of the body acceleration - * @param totalDist Total Distance to Run on this leg of Navigation - * @param dist2Go Remaining distance to Run on this leg of Navigation - * @param fromWP Origin WP - * @param toWP Destination WP - */ -static inline void mavlink_msg_slugs_navigation_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - float u_m,float phi_c,float theta_c,float psiDot_c,float ay_body,float totalDist,float dist2Go,uint8_t fromWP,uint8_t toWP) -{ - msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - - 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); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a slugs_navigation struct into a message * @@ -176,7 +139,19 @@ 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); - mavlink_msg_slugs_navigation_pack_chan_send(chan, msg, u_m, phi_c, theta_c, psiDot_c, ay_body, totalDist, dist2Go, fromWP, toWP); + msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; + + 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); } #endif diff --git a/thirdParty/mavlink/include/slugs/testsuite.h b/thirdParty/mavlink/include/slugs/testsuite.h index fc5d2aa12..ba8162cd1 100644 --- a/thirdParty/mavlink/include/slugs/testsuite.h +++ b/thirdParty/mavlink/include/slugs/testsuite.h @@ -46,8 +46,7 @@ static void mavlink_test_cpu_load(uint8_t system_id, uint8_t component_id) for (i=0; ic, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array); +} + +/** + * @brief Send a test_types message + * @param chan MAVLink channel to send the message + * * @param c char * @param s string * @param u8 uint8_t @@ -224,10 +235,11 @@ static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8 * @param f_array float_array * @param d_array double_array */ -static inline void mavlink_msg_test_types_pack_chan_send(mavlink_channel_t chan, - 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) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +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 @@ -255,56 +267,6 @@ static inline void mavlink_msg_test_types_pack_chan_send(mavlink_channel_t chan, mavlink_finalize_message_chan_send(msg, chan, 179, 103); } -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - -/** - * @brief Encode a test_types 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 test_types C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types) -{ - return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array); -} - -/** - * @brief Send a test_types message - * @param chan MAVLink channel to send the message - * - * @param c char - * @param s string - * @param u8 uint8_t - * @param u16 uint16_t - * @param u32 uint32_t - * @param u64 uint64_t - * @param s8 int8_t - * @param s16 int16_t - * @param s32 int32_t - * @param s64 int64_t - * @param f float - * @param d double - * @param u8_array uint8_t_array - * @param u16_array uint16_t_array - * @param u32_array uint32_t_array - * @param u64_array uint64_t_array - * @param s8_array int8_t_array - * @param s16_array int16_t_array - * @param s32_array int32_t_array - * @param s64_array int64_t_array - * @param f_array float_array - * @param d_array double_array - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -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); - mavlink_msg_test_types_pack_chan_send(chan, msg, c, s, u8, u16, u32, u64, s8, s16, s32, s64, f, d, u8_array, u16_array, u32_array, u64_array, s8_array, s16_array, s32_array, s64_array, f_array, d_array); -} #endif diff --git a/thirdParty/mavlink/include/test/testsuite.h b/thirdParty/mavlink/include/test/testsuite.h index 3b72ad009..4553fc730 100644 --- a/thirdParty/mavlink/include/test/testsuite.h +++ b/thirdParty/mavlink/include/test/testsuite.h @@ -65,8 +65,7 @@ static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id) for (i=0; imsgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - - 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] - - mavlink_finalize_message_chan_send(msg, chan, 32, 34); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a nav_filter_bias struct into a message * @@ -158,7 +125,17 @@ 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); - mavlink_msg_nav_filter_bias_pack_chan_send(chan, msg, usec, accel_0, accel_1, accel_2, gyro_0, gyro_1, gyro_2); + msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; + + 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] + + mavlink_finalize_message_chan_send(msg, chan, 32, 34); } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 16d5b1d30..aa46269c6 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -94,37 +94,6 @@ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 42, 71); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a radio_calibration message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param aileron Aileron setpoints: left, center, right - * @param elevator Elevator setpoints: nose down, center, nose up - * @param rudder Rudder setpoints: nose left, center, nose right - * @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode - * @param pitch Pitch curve setpoints (every 25%) - * @param throttle Throttle curve setpoints (every 25%) - */ -static inline void mavlink_msg_radio_calibration_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - const uint16_t *aileron,const uint16_t *elevator,const uint16_t *rudder,const uint16_t *gyro,const uint16_t *pitch,const uint16_t *throttle) -{ - msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - - 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%) - - mavlink_finalize_message_chan_send(msg, chan, 42, 71); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a radio_calibration struct into a message * @@ -154,7 +123,16 @@ 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); - mavlink_msg_radio_calibration_pack_chan_send(chan, msg, aileron, elevator, rudder, gyro, pitch, throttle); + msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; + + 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%) + + mavlink_finalize_message_chan_send(msg, chan, 42, 71); } #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 8dca0f90e..d6dfe47f1 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -71,31 +71,6 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_ return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 3, 15); } -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -/** - * @brief Pack a ualberta_sys_status message on a channel and send - * @param chan The MAVLink channel this message was sent over - * @param msg The MAVLink message to compress the data into - * @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM - * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM - * @param pilot Pilot mode, see UALBERTA_PILOT_MODE - */ -static inline void mavlink_msg_ualberta_sys_status_pack_chan_send(mavlink_channel_t chan, - mavlink_message_t* msg, - uint8_t mode,uint8_t nav_mode,uint8_t pilot) -{ - msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - - 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 - - mavlink_finalize_message_chan_send(msg, chan, 3, 15); -} -#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS - - /** * @brief Encode a ualberta_sys_status struct into a message * @@ -122,7 +97,13 @@ 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); - mavlink_msg_ualberta_sys_status_pack_chan_send(chan, msg, mode, nav_mode, pilot); + msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; + + 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 + + mavlink_finalize_message_chan_send(msg, chan, 3, 15); } #endif diff --git a/thirdParty/mavlink/include/ualberta/testsuite.h b/thirdParty/mavlink/include/ualberta/testsuite.h index 8cf86d30b..94ff8fc53 100644 --- a/thirdParty/mavlink/include/ualberta/testsuite.h +++ b/thirdParty/mavlink/include/ualberta/testsuite.h @@ -50,8 +50,7 @@ static void mavlink_test_nav_filter_bias(uint8_t system_id, uint8_t component_id for (i=0; i - - System is currently on ground. + + System is not ready to fly, booting, calibrating, etc. + + + System is allowed to be active, under assisted RC control. Level of stabilization depends on MAV_FLIGTH_MODE - + System is allowed to be active, under manual (RC) control, no stabilization - + System is allowed to be active, under autonomous control, manual setpoint - - System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. - - - The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. - - - RC control with stabilization; let go of the sticks and it will level. - - - The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. - - - Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. - - - Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. - - - Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_MODE_AUTO_VECTOR with user input) - - - Custom test mode, depends on individual autopilot. - - - Custom test mode, depends on individual autopilot. + + System is allowed to be active, under autonomous control and navigation (the trajectory is decided onboard and not pre-programmed by waypoints) - - Custom test mode, depends on individual autopilot. + + UNDEFINED mode. This solely depends on the autopilot - use with caution, intended for developers only. - - - System is currently on ground. Calibrating acceleraometers. - - - System is currently on ground. Calibrating Gyros. - - - System is currently on ground. Calibrating magnetometer. - - - System is currently on ground. Calibrating pressuresensors. - - - System is currently on ground. Waiting for GPS Lock. - - - System is currently on ground and ready. + + + System is currently on ground. - + No interaction of the autopilot with the actuator outputs, pure manual flight. - + System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. - + System is holding its current position and disabled the mission management. Loitering in mission mode is NOT the hold type, but still mission mode. - + System is navigating towards the next waypoint and following a mission script. - - System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_MODE_FBW_CURSOR_CONTROL + + System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_FLIGHT_MODE_FBW_CURSOR_CONTROL - + System is returning straight to home position. Once it reaches there it will hover or loiter at the autopilot's default holding settings. - + System is landing. Throttle is controlled by the autopilot. After getting closer than 30 meters, the course will lock to the current heading. Flare, throttle, flaps, gear, and other events can be scripted based on distance to landing point. - + System lost its position input and is in attitude / flight stabilization only. - + The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. - + RC control with stabilization; let go of the sticks and it will level. - + The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. - + Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. - + Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. - - Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_MODE_AUTO_VECTOR with user input) + + Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_FLIGHT_MODE_AUTO_VECTOR with user input) - + Custom test mode, depends on individual autopilot. - + Custom test mode, depends on individual autopilot. - + Custom test mode, depends on individual autopilot. @@ -208,9 +172,6 @@ System is active and might be already airborne. Motors are engaged. - - System is active and might be already airborne. Motors are engaged. - System is in a non-normal flight mode. It can however still navigate. @@ -550,7 +511,7 @@ Set system mode. Mode, as defined by ENUM MAV_MODE - Flight Mode, as defined by ENUM MAV_FLIGHT_MODE + Empty Empty Empty Empty @@ -739,7 +700,7 @@ Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) Autopilot type / class. defined in MAV_CLASS ENUM System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - Navigation mode, see MAV_MODE ENUM + Navigation mode, see MAV_FLIGHT_MODE ENUM System status flag, see MAV_STATUS ENUM System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation 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 @@ -748,19 +709,18 @@ The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. - 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 - 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 - 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 - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 10'000) should be always below 10'000 + 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 + 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 + 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 + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 Battery voltage, in millivolts (1 = 1 millivolt) - Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current + Battery current, in milliamperes (1 = 1 milliampere) Watts consumed from this battery since startup - Remaining battery energy: (0%: 0, 100%: 200%), -1: autopilot estimate the remaining battery - 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 - Autopilot-specific errors - Autopilot-specific errors + Remaining battery energy: (0%: 0, 100%: 255) + Dropped packets on all links (packets that were corrupted on reception on the MAV) + Dropped packets on all links (packets that were corrupted on reception) + Dropped packets on all links (packets that were corrupted on reception) + Dropped packets on all links (packets that were corrupted on reception) @@ -803,6 +763,11 @@ The system setting the mode The new mode + + Set the system navigation mode, as defined by enum MAV_FLIGHT_MODE. The navigation mode applies to the whole aircraft and thus all components. + The system setting the mode + The new navigation mode + Set the system safety mode, as defined by enum MAV_SAFETY. The navigation mode applies to the whole aircraft and thus all components. The system setting the mode @@ -848,7 +813,7 @@ GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 GPS ground speed (m/s * 100). If unknown, set to: 65535 - Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 Number of satellites visible. If unknown, set to 255 @@ -943,12 +908,11 @@ GPS HDOP GPS VDOP GPS ground speed - Course over ground (NOT heading, but direction of movement) in degrees, 0.0..359.99 degrees. If unknown, set to: -1 + Compass heading in degrees, 0..360 degrees Number of satellites visible The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. RC channel 1 value, in microseconds RC channel 2 value, in microseconds RC channel 3 value, in microseconds @@ -961,7 +925,6 @@ The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 - Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 @@ -974,7 +937,6 @@ The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. - Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. Servo output 1 value, in microseconds Servo output 2 value, in microseconds Servo output 3 value, in microseconds @@ -984,7 +946,7 @@ Servo output 7 value, in microseconds Servo output 8 value, in microseconds - + Message encoding a waypoint. This message is emitted to announce the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed @@ -1003,43 +965,43 @@ PARAM6 / y position: global: longitude PARAM7 / z position: global: altitude - + Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. System ID Component ID Sequence - + Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). System ID Component ID Sequence - + Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. Sequence - + Request the overall list of waypoints from the system/component. System ID Component ID - + This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. System ID Component ID Number of Waypoints in the Sequence - + Delete all waypoints at once. System ID Component ID - + A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. Sequence - + Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). System ID Component ID @@ -1082,13 +1044,6 @@ WGS84 Altitude in meters * 1000 (positive for up) Desired yaw angle in degrees * 100 - - Set the current global position setpoint. - WGS84 Latitude position in degrees * 1E7 - WGS84 Longitude position in degrees * 1E7 - WGS84 Altitude in meters * 1000 (positive for up) - Desired yaw angle in degrees * 100 - Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. System ID diff --git a/thirdParty/mavlink/message_definitions/common_future.xml b/thirdParty/mavlink/message_definitions/common_future.xml new file mode 100644 index 000000000..0023ef264 --- /dev/null +++ b/thirdParty/mavlink/message_definitions/common_future.xml @@ -0,0 +1,1399 @@ + + + 3 + + + Valid data types for parameter interface + + IEEE-754 floating point number + + + 32 bit unsigned integer (C99: uint32_t) + + + 32 bit signed integer (C99: int32_t) + + + + + Micro air vehicle / autopilot classes. This identifies the individual model. + + Generic autopilot, full support for everything + + + PIXHAWK autopilot, http://pixhawk.ethz.ch + + + SLUGS autopilot, http://slugsuav.soe.ucsc.edu + + + ArduPilotMega / ArduCopter, http://diydrones.com + + + OpenPilot, http://openpilot.org + + + Generic autopilot only supporting simple waypoints + + + Generic autopilot supporting waypoints and other simple navigation commands + + + Generic autopilot supporting the full mission command set + + + No valid autopilot, e.g. a GCS or other MAVLink component + + + PPZ UAV - http://nongnu.org/paparazzi + + + UAV Dev Board + + + FlexiPilot + + + + + MAV safety override. This is a top-level safety override. Independent of the general state machine this mode shall ensure the safe operation. the MAV_SAFETY state should ONLY be switchable in the MAV_STATE_STANDBY state and NOT in flight. + + MAV safety lock set to hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. + + + MAV safety set to disarmed. Motors are BLOCKED. Independent of the system mode and navigation mode all actuators are blocked. + + + MAV safety set to armed. Motors are ready / could be started. All actuators are active. + + + + + + System is currently on ground. + + + System is allowed to be active, under manual (RC) control, no stabilization + + + System is allowed to be active, under autonomous control, manual setpoint + + + System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. + + + The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. + + + RC control with stabilization; let go of the sticks and it will level. + + + The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. + + + Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. + + + Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. + + + Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_MODE_AUTO_VECTOR with user input) + + + Custom test mode, depends on individual autopilot. + + + Custom test mode, depends on individual autopilot. + + + Custom test mode, depends on individual autopilot. + + + + + System is currently on ground. Calibrating acceleraometers. + + + System is currently on ground. Calibrating Gyros. + + + System is currently on ground. Calibrating magnetometer. + + + System is currently on ground. Calibrating pressuresensors. + + + System is currently on ground. Waiting for GPS Lock. + + + System is currently on ground and ready. + + + No interaction of the autopilot with the actuator outputs, pure manual flight. + + + System is during takeoff, not in normal navigation mode yet. Once the plane is moving faster than a few m/s it will lock onto a heading and hold that heading until the desired altitude is reached. Throttle is limited by the RC throttle setting. + + + System is holding its current position and disabled the mission management. Loitering in mission mode is NOT the hold type, but still mission mode. + + + System is navigating towards the next waypoint and following a mission script. + + + System is flying at a defined course and speed. If the vector is not defined by an autonomous approach but constantly by a user, please use MAV_MODE_FBW_CURSOR_CONTROL + + + System is returning straight to home position. Once it reaches there it will hover or loiter at the autopilot's default holding settings. + + + System is landing. Throttle is controlled by the autopilot. After getting closer than 30 meters, the course will lock to the current heading. Flare, throttle, flaps, gear, and other events can be scripted based on distance to landing point. + + + System lost its position input and is in attitude / flight stabilization only. + + + The stick inputs commands angular rates. Only recommended for experienced pilots / acrobatic flight. + + + RC control with stabilization; let go of the sticks and it will level. + + + The autopilot will hold the roll and pitch specified by the control sticks. Throttle is manual. The plane / quadrotor will not roll past the limits set in the configuration of the autopilot. Great for new pilots learning to fly. + + + Requires airspeed sensor. The autopilot will hold the roll specified by the control sticks. Pitch input from the radio is converted to altitude error, which the autopilot will try and adjust to. Throttle is controlled by autopilot. This is the perfect mode to test your autopilot as your radio inout is substituted for the navigation controls. + + + Fly by wire mode, stabilizing roll, pitch, yaw and altitude. Typical altitude hold for quadrotors where the X / Y position is commanded with the roll / pitch stick. + + + Fly by wire mode, user is only directing the movement, but all flight control is autonomous (similar to MAV_MODE_AUTO_VECTOR with user input) + + + Custom test mode, depends on individual autopilot. + + + Custom test mode, depends on individual autopilot. + + + Custom test mode, depends on individual autopilot. + + + + + Uninitialized system, state is unknown. + + + System is booting up. + + + System is calibrating and not flight-ready. + + + System is grounded and on standby. It can be launched any time. + + + System is active and might be already airborne. Motors are engaged. + + + System is active and might be already airborne. Motors are engaged. + + + System is in a non-normal flight mode. It can however still navigate. + + + System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. + + + System just initialized its power-down sequence, will shut down now. + + + + + Generic micro air vehicle. + + + Fixed wing aircraft. + + + Quadrotor + + + Coaxial helicopter + + + Normal helicopter with tail rotor. + + + Ground installation + + + Operator control unit / ground control station + + + Airship, controlled + + + Free balloon, uncontrolled + + + Rocket + + + Ground rover + + + Surface vessel, boat, ship + + + Submarine + + + Hexarotor + + + Octorotor + + + Octorotor + + + Flapping wing + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) + + + Local coordinate frame, Z-up (x: north, y: east, z: down). + + + NOT a coordinate frame, indicates a mission command. + + + Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. + + + Local coordinate frame, Z-down (x: east, y: north, z: up) + + + + + + + + + + + + + + + + + + + + + + + + Commands to be executed by the MAV. They can be executed on user request, + or as part of a mission script. If the action is used in a mission, the parameter mapping + to the waypoint/mission message is as follows: + Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what + ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. + + Navigate to waypoint. + Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing) + Latitude + Longitude + Altitude + + + Loiter around this waypoint an unlimited amount of time + Empty + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X turns + Turns + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X seconds + Seconds (decimal) + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location + Empty + Empty + Empty + Desired yaw angle. + Latitude + Longitude + Altitude + + + Takeoff from ground / hand + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer + Latitude + Longitude + Altitude + + + Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + sensors such as cameras. + Region of intereset mode. (see MAV_ROI enum) + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + Control autonomous path planning on the MAV. + 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning + 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid + Empty + Yaw angle at goal, in compass degrees, [0..360] + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay mission state machine. + Delay in seconds (decimal) + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend at rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate (m/s) + Empty + Empty + Empty + Empty + Empty + Finish Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance (meters) + Empty + Empty + Empty + Empty + Empty + Empty + + + Reach a certain target angle. + target angle: [0-360], 0 is north + speed during yaw change:[deg per second] + direction: negative: counter clockwise, positive: clockwise [-1,1] + relative offset or absolute angle: [ 1,0] + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Set system mode. + Mode, as defined by ENUM MAV_MODE + Flight Mode, as defined by ENUM MAV_FLIGHT_MODE + Empty + Empty + Empty + Empty + Empty + + + Jump to the desired command in the mission list. Repeat this action only the specified number of times + Sequence number + Repeat count + Empty + Empty + Empty + Empty + Empty + + + Change speed and/or throttle set points. + Speed type (0=Airspeed, 1=Ground Speed) + Speed (m/s, -1 indicates no change) + Throttle ( Percent, -1 indicates no change) + Empty + Empty + Empty + Empty + + + Changes the home location either to the current location or a specified location. + Use current (1=use current location, 0=use specified location) + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. + Parameter number + Parameter value + Empty + Empty + Empty + Empty + Empty + + + Set a relay to a condition. + Relay number + Setting (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + Cycle a relay on and off for a desired number of cyles with a desired period. + Relay number + Cycle count + Cycle time (seconds, decimal) + Empty + Empty + Empty + Empty + + + Set a servo to a desired PWM value. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Empty + Empty + Empty + Empty + Empty + + + Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Cycle count + Cycle time (seconds) + Empty + Empty + Empty + + + Control onboard camera system. + Camera ID (-1 for all) + Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw + Transmission mode: 0: video stream, >0: single images every n seconds (decimal) + Recording: 0: disabled, 1: enabled compressed, 2: enabled raw + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the DO commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Trigger calibration. This command will be only accepted if in pre-flight mode. + Gyro calibration: 0: no, 1: yes + Magnetometer calibration: 0: no, 1: yes + Ground pressure: 0: no, 1: yes + Radio calibration: 0: no, 1: yes + Empty + Empty + Empty + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Reserved + Reserved + Empty + Empty + Empty + + + + Data stream IDs. A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. + + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + + + The ROI (region of interest) for the vehicle. This can be + be used by the vehicle for camera/vehicle attitude alignment (see + MAV_CMD_NAV_ROI). + + + No region of interest. + + + Point toward next waypoint. + + + Point toward given waypoint. + + + Point toward fixed location. + + + Point toward of given id. + + + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Autopilot type / class. defined in MAV_CLASS ENUM + System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + Navigation mode, see MAV_MODE ENUM + System status flag, see MAV_STATUS ENUM + System safety lock state, see MAV_SAFETY enum. Also indicates HIL operation + 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 + MAVLink version + + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + 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 + 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 + 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 + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 10'000) should be always below 10'000 + 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 + 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 + Autopilot-specific errors + Autopilot-specific errors + + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp of the master clock in microseconds since UNIX epoch. + Timestamp of the component clock since boot time in milliseconds. + + + + UTC date and time from GPS module + GPS UTC date ddmmyy + GPS UTC time hhmmss + + + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. + PING sequence + 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + Unix timestamp in microseconds + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + Set the system mode, as defined by enum MAV_MODE. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new mode + + + Set the system safety mode, as defined by enum MAV_SAFETY. The navigation mode applies to the whole aircraft and thus all components. + The system setting the mode + The new safety mode. The MAV will reject some mode changes during flight. + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id + Parameter index. Send -1 to use the param ID field as identifier + + + Request all parameters of this component. After his request, all parameters are emitted. + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. + Onboard parameter id + Onboard parameter value + Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. + System ID + Component ID + Onboard parameter id + Onboard parameter value + Onboard parameter type: 0: float, 1: uint8_t, 2: int8_t, 3: uint16_t, 4: int16_t, 5: uint32_t, 6: int32_t + + + The global position, as returned by the Global Positioning System (GPS). This is + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in 1E7 degrees + Longitude in 1E7 degrees + Altitude in 1E3 meters (millimeters) above MSL + GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + GPS ground speed (m/s * 100). If unknown, set to: 65535 + Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + Number of satellites visible. If unknown, set to 255 + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + Angular speed around X axis (millirad /sec) + Angular speed around Y axis (millirad /sec) + Angular speed around Z axis (millirad /sec) + X Magnetic field (milli tesla) + Y Magnetic field (milli tesla) + Z Magnetic field (milli tesla) + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (raw) + Differential pressure 1 (raw) + Differential pressure 2 (raw) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (hectopascal) + Differential pressure 1 (hectopascal) + Temperature measurement (0.01 degrees celsius) + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since unix epoch) + Latitude, in degrees + Longitude, in degrees + Absolute altitude, in meters + X Speed (in Latitude direction, positive: going north) + Y Speed (in Longitude direction, positive: going east) + Z Speed (in Altitude direction, positive: going up) + + + IMPORTANT: Please use GPS_RAW_INT for maximum precision. The max FLOAT resolution limits the resolution to about 1.8 m on some places on the earth. The global position, as returned by the Global Positioning System (GPS). This is + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in degrees + Longitude in degrees + Altitude in meters + GPS HDOP + GPS VDOP + GPS ground speed + Course over ground (NOT heading, but direction of movement) in degrees, 0.0..359.99 degrees. If unknown, set to: -1 + Number of satellites visible + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + + The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 + Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + Receive signal strength indicator, 0: 0%, 255: 100% + + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. + Servo output 1 value, in microseconds + Servo output 2 value, in microseconds + Servo output 3 value, in microseconds + Servo output 4 value, in microseconds + Servo output 5 value, in microseconds + Servo output 6 value, in microseconds + Servo output 7 value, in microseconds + Servo output 8 value, in microseconds + + + Message encoding a waypoint. This message is emitted to announce + the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed + + System ID + Component ID + Sequence + The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + false:0, true:1 + autocontinue to next wp + PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + PARAM5 / local: x position, global: latitude + PARAM6 / y position: global: longitude + PARAM7 / z position: global: altitude + + + Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. + System ID + Component ID + Sequence + + + Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. + Sequence + + + Request the overall list of waypoints from the system/component. + System ID + Component ID + + + This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of Waypoints in the Sequence + + + Delete all waypoints at once. + System ID + Component ID + + + A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + 0: OK, 1: Error + + + As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Component ID + global position * 1E7 + global position * 1E7 + global position * 1000 + + + Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position + Latitude (WGS84), expressed as * 1E7 + Longitude (WGS84), expressed as * 1E7 + Altitude(WGS84), expressed as * 1000 + + + Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. + System ID + Component ID + x position + y position + z position + Desired yaw angle + + + Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. + x position + y position + z position + Desired yaw angle + + + Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. + WGS84 Latitude position in degrees * 1E7 + WGS84 Longitude position in degrees * 1E7 + WGS84 Altitude in meters * 1000 (positive for up) + Desired yaw angle in degrees * 100 + + + Set the current global position setpoint. + WGS84 Latitude position in degrees * 1E7 + WGS84 Longitude position in degrees * 1E7 + WGS84 Altitude in meters * 1000 (positive for up) + Desired yaw angle in degrees * 100 + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 + + + Setpoint in roll, pitch, yaw currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 + + + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 + + + Outputs of the APM navigation controller. The primary use of this message is to check the response and signs + of the controller before actual flight and to assist with tuning controller parameters + + Current desired roll in degrees + Current desired pitch in degrees + Current desired heading in degrees + Bearing to current waypoint/target in degrees + Distance to active waypoint in meters + Current altitude error in meters + Current airspeed error in meters/second + Current crosstrack error on x-y plane in meters + + + The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. + x position error + y position error + z position error + roll error (radians) + pitch error (radians) + yaw error (radians) + x velocity + y velocity + z velocity + + + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested data stream + The requested interval between two messages of this type + 1 to start sending, 0 to stop sending. + + + The ID of the requested data stream + The requested interval between two messages of this type + 1 stream is enabled, 0 stream is stopped. + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + System ID + Component ID + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters), above MSL + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + + + Metrics typically displayed on a HUD for fixed wing aircraft + Current airspeed in m/s + Current ground speed in m/s + Current heading in degrees, in compass units (0..360, 0=north) + Current throttle setting in integer percent, 0 to 100 + Current altitude (MSL), in meters + Current climb rate in meters/second + + + Send a command with up to four parameters to the MAV + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + + + Send a command with up to four parameters to the MAV + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + Parameter 5, as defined by MAV_CMD enum. + Parameter 6, as defined by MAV_CMD enum. + Parameter 7, as defined by MAV_CMD enum. + + + Report status of a command. Includes feedback wether the command was executed + Current airspeed in m/s + 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + + + + Sent from simulation to autopilot. This packet is useful for high throughput + applications such as hardware in the loop simulations. + + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + + + Sent from autopilot to simulation. Hardware in the loop control outputs + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Control output -1 .. 1 + Control output -1 .. 1 + Control output -1 .. 1 + Throttle 0 .. 1 + Aux 1, -1 .. 1 + Aux 2, -1 .. 1 + Aux 3, -1 .. 1 + Aux 4, -1 .. 1 + System mode (MAV_MODE) + Navigation mode (MAV_NAV_MODE) + + + Sent from simulation to autopilot. The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + RC channel 9 value, in microseconds + RC channel 10 value, in microseconds + RC channel 11 value, in microseconds + RC channel 12 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + + Optical flow from a flow sensor (e.g. optical mouse sensor) + Timestamp (UNIX) + Sensor ID + Flow in pixels in x-sensor direction + Flow in pixels in y-sensor direction + Optical flow quality / confidence. 0: bad, 255: maximum quality + Ground distance in meters + + + + Send raw controller memory. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Starting address of the debug variables + Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + 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 + Memory contents at specified address + + + Name + Timestamp + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status, 0 = info message, 255 = critical fault + Status text message, without null termination character + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + index of debug variable + DEBUG value + + + diff --git a/thirdParty/mavlink/missionlib/mavlink_missionlib_data.h b/thirdParty/mavlink/missionlib/mavlink_missionlib_data.h new file mode 100644 index 000000000..c825fbcef --- /dev/null +++ b/thirdParty/mavlink/missionlib/mavlink_missionlib_data.h @@ -0,0 +1,58 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "mavlink.h" +#include + +/* MISSION LIB DATA STORAGE */ + +enum MAVLINK_PM_PARAMETERS +{ + MAVLINK_PM_PARAM_SYSTEM_ID, + MAVLINK_PM_PARAM_ATT_K_D, + MAVLINK_PM_MAX_PARAM_COUNT // VERY IMPORTANT! KEEP THIS AS LAST ENTRY +}; + + +/* DO NOT EDIT THIS FILE BELOW THIS POINT! */ + +#ifndef MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN +#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 +#endif + + +//extern void mavlink_pm_queued_send(); +struct mavlink_pm_storage { + char param_names[MAVLINK_PM_MAX_PARAM_COUNT][MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; ///< Parameter names + float param_values[MAVLINK_PM_MAX_PARAM_COUNT]; ///< Parameter values + uint16_t next_param; + uint16_t size; +}; + +typedef struct mavlink_pm_storage mavlink_pm_storage; + +void mavlink_pm_reset_params(mavlink_pm_storage* pm); diff --git a/thirdParty/mavlink/missionlib/mavlink_parameters.c b/thirdParty/mavlink/missionlib/mavlink_parameters.c index 8cd911e8a..7e7937bfe 100644 --- a/thirdParty/mavlink/missionlib/mavlink_parameters.c +++ b/thirdParty/mavlink/missionlib/mavlink_parameters.c @@ -86,7 +86,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess &tx_msg, pm.param_names[i], pm.param_values[i], - 0, + MAV_DATA_TYPE_FLOAT, pm.size, i); mavlink_missionlib_send_message(&tx_msg); @@ -94,7 +94,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess mavlink_msg_param_value_send(MAVLINK_COMM_0, pm.param_names[i], pm.param_values[i], - 0, + MAV_DATA_TYPE_FLOAT, pm.size, i); #endif @@ -133,7 +133,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess &tx_msg, pm.param_names[pm.next_param], pm.param_values[pm.next_param], - 0, + MAV_DATA_TYPE_FLOAT, pm.size, pm.next_param); mavlink_missionlib_send_message(&tx_msg); @@ -141,10 +141,10 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess mavlink_msg_param_value_send(MAVLINK_COMM_0, pm.param_names[pm.next_param], pm.param_values[pm.next_param], - 0, + MAV_DATA_TYPE_FLOAT, pm.size, pm.next_param); #endif pm.next_param++; } - } \ No newline at end of file + } diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c index 286aae2de..17c70a2d0 100644 --- a/thirdParty/mavlink/missionlib/waypoints.c +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -303,12 +303,8 @@ float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) return -1.f; } - -void mavlink_wpm_message_handler(const mavlink_message_t* msg) +void mavlink_wpm_loop() { - // Handle param messages - //paramClient->handleMAVLinkPacket(msg); - //check for timed-out operations uint64_t now = mavlink_missionlib_get_system_timestamp(); if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) @@ -334,7 +330,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_wpm_send_setpoint(wpm.current_active_wp_id); } - +} + +void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + uint64_t now = mavlink_missionlib_get_system_timestamp(); switch(msg->msgid) { case MAVLINK_MSG_ID_ATTITUDE: @@ -388,15 +388,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) float orbit = wp->param1; float dist; - if (wp->param2 == 0) - { - // FIXME segment distance - //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); - } - else - { +// if (wp->param2 == 0) +// { +// // FIXME segment distance +// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); +// } +// else +// { dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); - } +// } if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) { diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h index a0f2aa6d1..41e15043a 100644 --- a/thirdParty/mavlink/missionlib/waypoints.h +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -55,9 +55,11 @@ enum MAVLINK_WPM_CODES #define MAVLINK_WPM_MAX_WP_COUNT 15 #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 +#ifndef MAVLINK_WPM_TEXT_FEEDBACK +#define MAVLINK_WPM_TEXT_FEEDBACK 0 ///< Report back status information as text +#endif +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000 ///< Protocol communication timeout in milliseconds +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000 ///< When to send a new setpoint #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 @@ -91,5 +93,5 @@ struct mavlink_wpm_storage { typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage* state); - +void mavlink_wpm_loop(); void mavlink_wpm_message_handler(const mavlink_message_t* msg); -- 2.22.0