From 8fef3ca0bd95b4be7d58c2f6f2b357dc95bebb25 Mon Sep 17 00:00:00 2001 From: lm Date: Fri, 12 Aug 2011 22:51:09 +0200 Subject: [PATCH] Updated embedded MAVLink --- thirdParty/mavlink/include/common/common.h | 21 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../include/common/mavlink_msg_attitude.h | 80 +- .../include/common/mavlink_msg_auth_key.h | 32 +- .../mavlink/include/common/mavlink_msg_boot.h | 32 +- .../mavlink_msg_change_operator_control.h | 56 +- .../mavlink_msg_change_operator_control_ack.h | 48 +- .../include/common/mavlink_msg_cmd_ack.h | 40 +- .../include/common/mavlink_msg_command.h | 90 +- .../include/common/mavlink_msg_command_ack.h | 40 +- .../common/mavlink_msg_control_status.h | 90 +- .../include/common/mavlink_msg_debug.h | 40 +- .../include/common/mavlink_msg_debug_vect.h | 64 +- .../include/common/mavlink_msg_full_state.h | 154 ++-- .../common/mavlink_msg_global_position.h | 80 +- .../common/mavlink_msg_global_position_int.h | 80 +- .../common/mavlink_msg_gps_local_origin_set.h | 48 +- .../include/common/mavlink_msg_gps_raw.h | 104 ++- .../include/common/mavlink_msg_gps_raw_int.h | 104 ++- .../mavlink_msg_gps_set_global_origin.h | 64 +- .../include/common/mavlink_msg_gps_status.h | 72 +- .../include/common/mavlink_msg_heartbeat.h | 48 +- .../common/mavlink_msg_local_position.h | 80 +- .../mavlink_msg_local_position_setpoint.h | 56 +- .../mavlink_msg_local_position_setpoint_set.h | 72 +- .../common/mavlink_msg_manual_control.h | 96 +- .../include/common/mavlink_msg_memory_vect.h | 56 +- .../common/mavlink_msg_named_value_float.h | 40 +- .../common/mavlink_msg_named_value_int.h | 40 +- .../mavlink_msg_nav_controller_output.h | 90 +- .../common/mavlink_msg_param_request_list.h | 40 +- .../common/mavlink_msg_param_request_read.h | 56 +- .../include/common/mavlink_msg_param_set.h | 56 +- .../include/common/mavlink_msg_param_value.h | 56 +- .../mavlink/include/common/mavlink_msg_ping.h | 56 +- .../common/mavlink_msg_position_target.h | 56 +- .../include/common/mavlink_msg_raw_imu.h | 104 ++- .../include/common/mavlink_msg_raw_pressure.h | 64 +- .../common/mavlink_msg_rc_channels_override.h | 104 ++- .../common/mavlink_msg_rc_channels_raw.h | 96 +- .../common/mavlink_msg_rc_channels_scaled.h | 96 +- .../common/mavlink_msg_request_data_stream.h | 64 +- ...msg_roll_pitch_yaw_speed_thrust_setpoint.h | 194 ++++ ...vlink_msg_roll_pitch_yaw_thrust_setpoint.h | 194 ++++ .../common/mavlink_msg_safety_allowed_area.h | 80 +- .../mavlink_msg_safety_set_allowed_area.h | 96 +- .../include/common/mavlink_msg_scaled_imu.h | 104 ++- .../common/mavlink_msg_scaled_pressure.h | 56 +- .../common/mavlink_msg_servo_output_raw.h | 90 +- .../include/common/mavlink_msg_set_altitude.h | 40 +- .../include/common/mavlink_msg_set_mode.h | 40 +- .../include/common/mavlink_msg_set_nav_mode.h | 40 +- ...link_msg_set_roll_pitch_yaw_speed_thrust.h | 212 +++++ .../mavlink_msg_set_roll_pitch_yaw_thrust.h | 212 +++++ .../common/mavlink_msg_state_correction.h | 96 +- .../include/common/mavlink_msg_statustext.h | 40 +- .../include/common/mavlink_msg_sys_status.h | 80 +- .../include/common/mavlink_msg_system_time.h | 32 +- .../common/mavlink_msg_system_time_utc.h | 40 +- .../include/common/mavlink_msg_vfr_hud.h | 72 +- .../include/common/mavlink_msg_waypoint.h | 136 ++- .../include/common/mavlink_msg_waypoint_ack.h | 48 +- .../common/mavlink_msg_waypoint_clear_all.h | 40 +- .../common/mavlink_msg_waypoint_count.h | 48 +- .../common/mavlink_msg_waypoint_current.h | 32 +- .../common/mavlink_msg_waypoint_reached.h | 32 +- .../common/mavlink_msg_waypoint_request.h | 48 +- .../mavlink_msg_waypoint_request_list.h | 40 +- .../common/mavlink_msg_waypoint_set_current.h | 48 +- thirdParty/mavlink/include/mavlink_checksum.h | 339 +++---- thirdParty/mavlink/include/mavlink_data.h | 40 +- thirdParty/mavlink/include/mavlink_options.h | 237 ++--- thirdParty/mavlink/include/mavlink_protocol.h | 852 +++++++++--------- thirdParty/mavlink/include/mavlink_types.h | 229 ++--- .../mavlink/message_definitions/common.xml | 28 +- 75 files changed, 3717 insertions(+), 2965 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_checksum.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_data.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_options.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_protocol.h mode change 100755 => 100644 thirdParty/mavlink/include/mavlink_types.h diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 8ce7b21f8..2364eb3ec 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Thursday, August 11 2011, 07:25 UTC + * Generated on Friday, August 12 2011, 20:25 UTC */ #ifndef COMMON_H #define COMMON_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_COMMON @@ -273,12 +273,10 @@ enum MAV_CMD #include "./mavlink_msg_control_status.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" -#include "./mavlink_msg_set_roll_pitch_yaw.h" -#include "./mavlink_msg_set_roll_pitch_yaw_speed.h" -#include "./mavlink_msg_roll_pitch_yaw_setpoint.h" -#include "./mavlink_msg_roll_pitch_yaw_speed_setpoint.h" -#include "./mavlink_msg_attitude_controller_output.h" -#include "./mavlink_msg_position_controller_output.h" +#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" +#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" #include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_position_target.h" #include "./mavlink_msg_state_correction.h" @@ -302,7 +300,12 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 79, 252, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 } + +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 7de59d0c6..9c3399f45 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Thursday, August 11 2011, 07:25 UTC + * Generated on Friday, August 12 2011, 20:25 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 8c32cd48c..c0d0c7350 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_ATTITUDE 30 #define MAVLINK_MSG_ID_ATTITUDE_LEN 32 #define MAVLINK_MSG_30_LEN 32 +#define MAVLINK_MSG_ID_ATTITUDE_KEY 0xF3 +#define MAVLINK_MSG_30_KEY 0xF3 typedef struct __mavlink_attitude_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) } mavlink_attitude_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_attitude_pack(uint8_t system_id, uint8_t comp mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_attitude_pack_chan(uint8_t system_id, uint8_t mavlink_attitude_t *p = (mavlink_attitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co return mavlink_msg_attitude_pack(system_id, component_id, msg, attitude->usec, attitude->roll, attitude->pitch, attitude->yaw, attitude->rollspeed, attitude->pitchspeed, attitude->yawspeed); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a attitude message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { mavlink_header_t hdr; mavlink_attitude_t payload; - uint16_t checksum; - mavlink_attitude_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ATTITUDE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.roll = roll; // float:Roll angle (rad) + payload.pitch = pitch; // float:Pitch angle (rad) + payload.yaw = yaw; // float:Yaw angle (rad) + payload.rollspeed = rollspeed; // float:Roll angular speed (rad/s) + payload.pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + payload.yawspeed = yawspeed; // float:Yaw angular speed (rad/s) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_ATTITUDE_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t us mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF3, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index 36366e2dc..e75ffd1b7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_AUTH_KEY 7 #define MAVLINK_MSG_ID_AUTH_KEY_LEN 32 #define MAVLINK_MSG_7_LEN 32 +#define MAVLINK_MSG_ID_AUTH_KEY_KEY 0xBA +#define MAVLINK_MSG_7_KEY 0xBA typedef struct __mavlink_auth_key_t { - char key[32]; ///< key + char key[32]; ///< key } mavlink_auth_key_t; #define MAVLINK_MSG_AUTH_KEY_FIELD_KEY_LEN 32 @@ -25,7 +27,7 @@ static inline uint16_t mavlink_msg_auth_key_pack(uint8_t system_id, uint8_t comp mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - memcpy(p->key, key, sizeof(p->key)); // char[32]:key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTH_KEY_LEN); } @@ -44,7 +46,7 @@ static inline uint16_t mavlink_msg_auth_key_pack_chan(uint8_t system_id, uint8_t mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUTH_KEY; - memcpy(p->key, key, sizeof(p->key)); // char[32]:key + memcpy(p->key, key, sizeof(p->key)); // char[32]:key return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTH_KEY_LEN); } @@ -62,23 +64,21 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co return mavlink_msg_auth_key_pack(system_id, component_id, msg, auth_key->key); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a auth_key message * @param chan MAVLink channel to send the message * * @param key key */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) { mavlink_header_t hdr; mavlink_auth_key_t payload; - uint16_t checksum; - mavlink_auth_key_t *p = &payload; - memcpy(p->key, key, sizeof(p->key)); // char[32]:key + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_AUTH_KEY_LEN ) + memcpy(payload.key, key, sizeof(payload.key)); // char[32]:key hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; @@ -89,14 +89,12 @@ static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xBA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 9051bb199..0c043e83a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_BOOT 1 #define MAVLINK_MSG_ID_BOOT_LEN 4 #define MAVLINK_MSG_1_LEN 4 +#define MAVLINK_MSG_ID_BOOT_KEY 0xF9 +#define MAVLINK_MSG_1_KEY 0xF9 typedef struct __mavlink_boot_t { - uint32_t version; ///< The onboard software version + uint32_t version; ///< The onboard software version } mavlink_boot_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_boot_pack(uint8_t system_id, uint8_t componen mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - p->version = version; // uint32_t:The onboard software version + p->version = version; // uint32_t:The onboard software version return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BOOT_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_boot_pack_chan(uint8_t system_id, uint8_t com mavlink_boot_t *p = (mavlink_boot_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BOOT; - p->version = version; // uint32_t:The onboard software version + p->version = version; // uint32_t:The onboard software version return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BOOT_LEN); } @@ -61,23 +63,21 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon return mavlink_msg_boot_pack(system_id, component_id, msg, boot->version); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a boot message * @param chan MAVLink channel to send the message * * @param version The onboard software version */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { mavlink_header_t hdr; mavlink_boot_t payload; - uint16_t checksum; - mavlink_boot_t *p = &payload; - p->version = version; // uint32_t:The onboard software version + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_BOOT_LEN ) + payload.version = version; // uint32_t:The onboard software version hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_BOOT_LEN; @@ -88,14 +88,12 @@ static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t versio mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 9b1a2e795..d78480cf5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL 5 #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN 28 #define MAVLINK_MSG_5_LEN 28 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_KEY 0x7E +#define MAVLINK_MSG_5_KEY 0x7E typedef struct __mavlink_change_operator_control_t { - uint8_t target_system; ///< System the GCS requests control for - uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + uint8_t target_system; ///< System the GCS requests control for + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t version; ///< 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + char passkey[25]; ///< Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" } mavlink_change_operator_control_t; #define MAVLINK_MSG_CHANGE_OPERATOR_CONTROL_FIELD_PASSKEY_LEN 25 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_change_operator_control_pack(uint8_t system_i mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - p->target_system = target_system; // uint8_t:System the GCS requests control for - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_change_operator_control_pack_chan(uint8_t sys mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - p->target_system = target_system; // uint8_t:System the GCS requests control for - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + p->target_system = target_system; // uint8_t:System the GCS requests control for + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN); } @@ -77,6 +79,8 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system return mavlink_msg_change_operator_control_pack(system_id, component_id, msg, change_operator_control->target_system, change_operator_control->control_request, change_operator_control->version, change_operator_control->passkey); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a change_operator_control message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { mavlink_header_t hdr; mavlink_change_operator_control_t payload; - uint16_t checksum; - mavlink_change_operator_control_t *p = &payload; - p->target_system = target_system; // uint8_t:System the GCS requests control for - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN ) + payload.target_system = target_system; // uint8_t:System the GCS requests control for + payload.control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + payload.version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + memcpy(payload.passkey, passkey, sizeof(payload.passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x7E, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 8e9315675..2afdd8077 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 @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK 6 #define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN 3 #define MAVLINK_MSG_6_LEN 3 +#define MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_KEY 0x75 +#define MAVLINK_MSG_6_KEY 0x75 typedef struct __mavlink_change_operator_control_ack_t { - uint8_t gcs_system_id; ///< ID of the GCS this message - uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV - uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + uint8_t gcs_system_id; ///< ID of the GCS this message + uint8_t control_request; ///< 0: request control of this MAV, 1: Release control of this MAV + uint8_t ack; ///< 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control } mavlink_change_operator_control_ack_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack(uint8_t syst mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_pack_chan(uint8_t mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy return mavlink_msg_change_operator_control_ack_pack(system_id, component_id, msg, change_operator_control_ack->gcs_system_id, change_operator_control_ack->control_request, change_operator_control_ack->ack); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a change_operator_control_ack message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy * @param control_request 0: request control of this MAV, 1: Release control of this MAV * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { mavlink_header_t hdr; mavlink_change_operator_control_ack_t payload; - uint16_t checksum; - mavlink_change_operator_control_ack_t *p = &payload; - p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN ) + payload.gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message + payload.control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV + payload.ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x75, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h index b2e760965..acfc50312 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_cmd_ack.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_CMD_ACK 9 #define MAVLINK_MSG_ID_CMD_ACK_LEN 2 #define MAVLINK_MSG_9_LEN 2 +#define MAVLINK_MSG_ID_CMD_ACK_KEY 0x90 +#define MAVLINK_MSG_9_KEY 0x90 typedef struct __mavlink_cmd_ack_t { - uint8_t cmd; ///< The MAV_CMD ID - uint8_t result; ///< 0: Action DENIED, 1: Action executed + uint8_t cmd; ///< The MAV_CMD ID + uint8_t result; ///< 0: Action DENIED, 1: Action executed } mavlink_cmd_ack_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_cmd_ack_pack(uint8_t system_id, uint8_t compo mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CMD_ACK; - p->cmd = cmd; // uint8_t:The MAV_CMD ID - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CMD_ACK_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_cmd_ack_pack_chan(uint8_t system_id, uint8_t mavlink_cmd_ack_t *p = (mavlink_cmd_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CMD_ACK; - p->cmd = cmd; // uint8_t:The MAV_CMD ID - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + p->cmd = cmd; // uint8_t:The MAV_CMD ID + p->result = result; // uint8_t:0: Action DENIED, 1: Action executed return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CMD_ACK_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_cmd_ack_encode(uint8_t system_id, uint8_t com return mavlink_msg_cmd_ack_pack(system_id, component_id, msg, cmd_ack->cmd, cmd_ack->result); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a cmd_ack message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_cmd_ack_encode(uint8_t system_id, uint8_t com * @param cmd The MAV_CMD ID * @param result 0: Action DENIED, 1: Action executed */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_cmd_ack_send(mavlink_channel_t chan, uint8_t cmd, uint8_t result) { mavlink_header_t hdr; mavlink_cmd_ack_t payload; - uint16_t checksum; - mavlink_cmd_ack_t *p = &payload; - p->cmd = cmd; // uint8_t:The MAV_CMD ID - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CMD_ACK_LEN ) + payload.cmd = cmd; // uint8_t:The MAV_CMD ID + payload.result = result; // uint8_t:0: Action DENIED, 1: Action executed hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CMD_ACK_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_cmd_ack_send(mavlink_channel_t chan, uint8_t cmd, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x90, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index 7b8afd94b..72cd1bebc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_COMMAND 75 #define MAVLINK_MSG_ID_COMMAND_LEN 20 #define MAVLINK_MSG_75_LEN 20 +#define MAVLINK_MSG_ID_COMMAND_KEY 0x14 +#define MAVLINK_MSG_75_KEY 0x14 typedef struct __mavlink_command_t { - float param1; ///< Parameter 1, as defined by MAV_CMD enum. - float param2; ///< Parameter 2, as defined by MAV_CMD enum. - float param3; ///< Parameter 3, as defined by MAV_CMD enum. - float param4; ///< Parameter 4, as defined by MAV_CMD enum. - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + float param1; ///< Parameter 1, as defined by MAV_CMD enum. + float param2; ///< Parameter 2, as defined by MAV_CMD enum. + float param3; ///< Parameter 3, as defined by MAV_CMD enum. + float param4; ///< Parameter 4, as defined by MAV_CMD enum. + uint8_t target_system; ///< System which should execute the command + uint8_t target_component; ///< Component which should execute the command, 0 for all components + uint8_t command; ///< Command ID, as defined by MAV_CMD enum. + uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) } mavlink_command_t; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t compo mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - p->target_system = target_system; // uint8_t:System which should execute the command - p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_command_pack_chan(uint8_t system_id, uint8_t mavlink_command_t *p = (mavlink_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND; - p->target_system = target_system; // uint8_t:System which should execute the command - p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + p->target_system = target_system; // uint8_t:System which should execute the command + p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_LEN); } @@ -96,6 +98,8 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com return mavlink_msg_command_pack(system_id, component_id, msg, command->target_system, command->target_component, command->command, command->confirmation, command->param1, command->param2, command->param3, command->param4); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a command message * @param chan MAVLink channel to send the message @@ -109,24 +113,20 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param3 Parameter 3, as defined by MAV_CMD enum. * @param param4 Parameter 4, as defined by MAV_CMD enum. */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { mavlink_header_t hdr; mavlink_command_t payload; - uint16_t checksum; - mavlink_command_t *p = &payload; - - p->target_system = target_system; // uint8_t:System which should execute the command - p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_COMMAND_LEN ) + payload.target_system = target_system; // uint8_t:System which should execute the command + payload.target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components + payload.command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. + payload.confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + payload.param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. + payload.param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. + payload.param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. + payload.param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_COMMAND_LEN; @@ -137,14 +137,12 @@ static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t targ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x14, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index c7e4a4b0f..899b12507 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_COMMAND_ACK 76 #define MAVLINK_MSG_ID_COMMAND_ACK_LEN 8 #define MAVLINK_MSG_76_LEN 8 +#define MAVLINK_MSG_ID_COMMAND_ACK_KEY 0x16 +#define MAVLINK_MSG_76_KEY 0x16 typedef struct __mavlink_command_ack_t { - float command; ///< Current airspeed in m/s - float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + float command; ///< Current airspeed in m/s + float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION } mavlink_command_ack_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - p->command = command; // float:Current airspeed in m/s - p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK; - p->command = command; // float:Current airspeed in m/s - p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + p->command = command; // float:Current airspeed in m/s + p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a command_ack message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t * @param command Current airspeed in m/s * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { mavlink_header_t hdr; mavlink_command_ack_t payload; - uint16_t checksum; - mavlink_command_ack_t *p = &payload; - p->command = command; // float:Current airspeed in m/s - p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_COMMAND_ACK_LEN ) + payload.command = command; // float:Current airspeed in m/s + payload.result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float co mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x16, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 8974f07d0..bc19b9c84 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_CONTROL_STATUS 52 #define MAVLINK_MSG_ID_CONTROL_STATUS_LEN 8 #define MAVLINK_MSG_52_LEN 8 +#define MAVLINK_MSG_ID_CONTROL_STATUS_KEY 0xF9 +#define MAVLINK_MSG_52_KEY 0xF9 typedef struct __mavlink_control_status_t { - uint8_t position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - uint8_t vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - uint8_t gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - uint8_t ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent - uint8_t control_att; ///< 0: Attitude control disabled, 1: enabled - uint8_t control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled - uint8_t control_pos_z; ///< 0: Z position control disabled, 1: enabled - uint8_t control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled + uint8_t position_fix; ///< Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + uint8_t vision_fix; ///< Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + uint8_t gps_fix; ///< GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + uint8_t ahrs_health; ///< Attitude estimation health: 0: poor, 255: excellent + uint8_t control_att; ///< 0: Attitude control disabled, 1: enabled + uint8_t control_pos_xy; ///< 0: X, Y position control disabled, 1: enabled + uint8_t control_pos_z; ///< 0: Z position control disabled, 1: enabled + uint8_t control_pos_yaw; ///< 0: Yaw angle control disabled, 1: enabled } mavlink_control_status_t; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_control_status_pack(uint8_t system_id, uint8_ mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent - p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled - p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled - p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled - p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_control_status_pack_chan(uint8_t system_id, u mavlink_control_status_t *p = (mavlink_control_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent - p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled - p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled - p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled - p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN); } @@ -96,6 +98,8 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint return mavlink_msg_control_status_pack(system_id, component_id, msg, control_status->position_fix, control_status->vision_fix, control_status->gps_fix, control_status->ahrs_health, control_status->control_att, control_status->control_pos_xy, control_status->control_pos_z, control_status->control_pos_yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a control_status message * @param chan MAVLink channel to send the message @@ -109,24 +113,20 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint * @param control_pos_z 0: Z position control disabled, 1: enabled * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { mavlink_header_t hdr; mavlink_control_status_t payload; - uint16_t checksum; - mavlink_control_status_t *p = &payload; - - p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent - p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled - p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled - p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled - p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CONTROL_STATUS_LEN ) + payload.position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + payload.vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + payload.gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + payload.ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent + payload.control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled + payload.control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled + payload.control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled + payload.control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; @@ -137,14 +137,12 @@ static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index 419ef33e8..2de388fc5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_DEBUG 255 #define MAVLINK_MSG_ID_DEBUG_LEN 5 #define MAVLINK_MSG_255_LEN 5 +#define MAVLINK_MSG_ID_DEBUG_KEY 0x54 +#define MAVLINK_MSG_255_KEY 0x54 typedef struct __mavlink_debug_t { - float value; ///< DEBUG value - uint8_t ind; ///< index of debug variable + float value; ///< DEBUG value + uint8_t ind; ///< index of debug variable } mavlink_debug_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_debug_pack(uint8_t system_id, uint8_t compone mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_debug_pack_chan(uint8_t system_id, uint8_t co mavlink_debug_t *p = (mavlink_debug_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value + p->ind = ind; // uint8_t:index of debug variable + p->value = value; // float:DEBUG value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo return mavlink_msg_debug_pack(system_id, component_id, msg, debug->ind, debug->value); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a debug message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo * @param ind index of debug variable * @param value DEBUG value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { mavlink_header_t hdr; mavlink_debug_t payload; - uint16_t checksum; - mavlink_debug_t *p = &payload; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DEBUG_LEN ) + payload.ind = ind; // uint8_t:index of debug variable + payload.value = value; // float:DEBUG value hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_DEBUG_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, f mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x54, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index 53ef8bd11..a0565e755 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_DEBUG_VECT 251 #define MAVLINK_MSG_ID_DEBUG_VECT_LEN 30 #define MAVLINK_MSG_251_LEN 30 +#define MAVLINK_MSG_ID_DEBUG_VECT_KEY 0x2B +#define MAVLINK_MSG_251_KEY 0x2B typedef struct __mavlink_debug_vect_t { - uint64_t usec; ///< Timestamp - float x; ///< x - float y; ///< y - float z; ///< z - char name[10]; ///< Name + uint64_t usec; ///< Timestamp + float x; ///< x + float y; ///< y + float z; ///< z + char name[10]; ///< Name } mavlink_debug_vect_t; #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 @@ -33,11 +35,11 @@ static inline uint16_t mavlink_msg_debug_vect_pack(uint8_t system_id, uint8_t co mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name - p->usec = usec; // uint64_t:Timestamp - p->x = x; // float:x - p->y = y; // float:y - p->z = z; // float:z + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } @@ -60,11 +62,11 @@ static inline uint16_t mavlink_msg_debug_vect_pack_chan(uint8_t system_id, uint8 mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DEBUG_VECT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name - p->usec = usec; // uint64_t:Timestamp - p->x = x; // float:x - p->y = y; // float:y - p->z = z; // float:z + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name + p->usec = usec; // uint64_t:Timestamp + p->x = x; // float:x + p->y = y; // float:y + p->z = z; // float:z return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN); } @@ -82,6 +84,8 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t return mavlink_msg_debug_vect_pack(system_id, component_id, msg, debug_vect->name, debug_vect->usec, debug_vect->x, debug_vect->y, debug_vect->z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a debug_vect message * @param chan MAVLink channel to send the message @@ -92,21 +96,17 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param y y * @param z z */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) { mavlink_header_t hdr; mavlink_debug_vect_t payload; - uint16_t checksum; - mavlink_debug_vect_t *p = &payload; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name - p->usec = usec; // uint64_t:Timestamp - p->x = x; // float:x - p->y = y; // float:y - p->z = z; // float:z + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DEBUG_VECT_LEN ) + memcpy(payload.name, name, sizeof(payload.name)); // char[10]:Name + payload.usec = usec; // uint64_t:Timestamp + payload.x = x; // float:x + payload.y = y; // float:y + payload.z = z; // float:z hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; @@ -117,14 +117,12 @@ static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const cha mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x2B, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h index 263d3e967..9cacfd970 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h @@ -3,25 +3,27 @@ #define MAVLINK_MSG_ID_FULL_STATE 67 #define MAVLINK_MSG_ID_FULL_STATE_LEN 56 #define MAVLINK_MSG_67_LEN 56 +#define MAVLINK_MSG_ID_FULL_STATE_KEY 0x26 +#define MAVLINK_MSG_67_KEY 0x26 typedef struct __mavlink_full_state_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float roll; ///< Roll angle (rad) - float pitch; ///< Pitch angle (rad) - float yaw; ///< Yaw angle (rad) - float rollspeed; ///< Roll angular speed (rad/s) - float pitchspeed; ///< Pitch angular speed (rad/s) - float yawspeed; ///< Yaw angular speed (rad/s) - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) - int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) } mavlink_full_state_t; @@ -54,22 +56,22 @@ static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t co mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_FULL_STATE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FULL_STATE_LEN); } @@ -103,22 +105,22 @@ static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8 mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_FULL_STATE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FULL_STATE_LEN); } @@ -136,6 +138,8 @@ static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a full_state message * @param chan MAVLink channel to send the message @@ -157,32 +161,28 @@ static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t * @param yacc Y acceleration (mg) * @param zacc Z acceleration (mg) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) { mavlink_header_t hdr; mavlink_full_state_t payload; - uint16_t checksum; - mavlink_full_state_t *p = &payload; - - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_FULL_STATE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.roll = roll; // float:Roll angle (rad) + payload.pitch = pitch; // float:Pitch angle (rad) + payload.yaw = yaw; // float:Yaw angle (rad) + payload.rollspeed = rollspeed; // float:Roll angular speed (rad/s) + payload.pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + payload.yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + payload.lat = lat; // int32_t:Latitude, expressed as * 1E7 + payload.lon = lon; // int32_t:Longitude, expressed as * 1E7 + payload.alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + payload.vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + payload.vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + payload.vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + payload.xacc = xacc; // int16_t:X acceleration (mg) + payload.yacc = yacc; // int16_t:Y acceleration (mg) + payload.zacc = zacc; // int16_t:Z acceleration (mg) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_FULL_STATE_LEN; @@ -193,14 +193,12 @@ static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x26, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index 26e2d8a13..c3a1accfa 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_GLOBAL_POSITION 33 #define MAVLINK_MSG_ID_GLOBAL_POSITION_LEN 32 #define MAVLINK_MSG_33_LEN 32 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_KEY 0x15 +#define MAVLINK_MSG_33_KEY 0x15 typedef struct __mavlink_global_position_t { - uint64_t usec; ///< Timestamp (microseconds since unix epoch) - float lat; ///< Latitude, in degrees - float lon; ///< Longitude, in degrees - float alt; ///< Absolute altitude, in meters - float vx; ///< X Speed (in Latitude direction, positive: going north) - float vy; ///< Y Speed (in Longitude direction, positive: going east) - float vz; ///< Z Speed (in Altitude direction, positive: going up) + uint64_t usec; ///< Timestamp (microseconds since unix epoch) + float lat; ///< Latitude, in degrees + float lon; ///< Longitude, in degrees + float alt; ///< Absolute altitude, in meters + float vx; ///< X Speed (in Latitude direction, positive: going north) + float vy; ///< Y Speed (in Longitude direction, positive: going east) + float vz; ///< Z Speed (in Altitude direction, positive: going up) } mavlink_global_position_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_global_position_pack(uint8_t system_id, uint8 mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) - p->lat = lat; // float:Latitude, in degrees - p->lon = lon; // float:Longitude, in degrees - p->alt = alt; // float:Absolute altitude, in meters - p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) - p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) - p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_global_position_pack_chan(uint8_t system_id, mavlink_global_position_t *p = (mavlink_global_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) - p->lat = lat; // float:Latitude, in degrees - p->lon = lon; // float:Longitude, in degrees - p->alt = alt; // float:Absolute altitude, in meters - p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) - p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) - p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + p->lat = lat; // float:Latitude, in degrees + p->lon = lon; // float:Longitude, in degrees + p->alt = alt; // float:Absolute altitude, in meters + p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) + p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin return mavlink_msg_global_position_pack(system_id, component_id, msg, global_position->usec, global_position->lat, global_position->lon, global_position->alt, global_position->vx, global_position->vy, global_position->vz); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a global_position message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin * @param vy Y Speed (in Longitude direction, positive: going east) * @param vz Z Speed (in Altitude direction, positive: going up) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { mavlink_header_t hdr; mavlink_global_position_t payload; - uint16_t checksum; - mavlink_global_position_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) - p->lat = lat; // float:Latitude, in degrees - p->lon = lon; // float:Longitude, in degrees - p->alt = alt; // float:Absolute altitude, in meters - p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) - p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) - p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GLOBAL_POSITION_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) + payload.lat = lat; // float:Latitude, in degrees + payload.lon = lon; // float:Longitude, in degrees + payload.alt = alt; // float:Absolute altitude, in meters + payload.vx = vx; // float:X Speed (in Latitude direction, positive: going north) + payload.vy = vy; // float:Y Speed (in Longitude direction, positive: going east) + payload.vz = vz; // float:Z Speed (in Altitude direction, positive: going up) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x15, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 9dbc636e2..218a757c2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT 73 #define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN 20 #define MAVLINK_MSG_73_LEN 20 +#define MAVLINK_MSG_ID_GLOBAL_POSITION_INT_KEY 0xD4 +#define MAVLINK_MSG_73_KEY 0xD4 typedef struct __mavlink_global_position_int_t { - int32_t lat; ///< Latitude, expressed as * 1E7 - int32_t lon; ///< Longitude, expressed as * 1E7 - int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL - int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 - int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 - int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 - uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters), above MSL + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 } mavlink_global_position_int_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_global_position_int_pack(uint8_t system_id, u mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_global_position_int_pack_chan(uint8_t system_ mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, return mavlink_msg_global_position_int_pack(system_id, component_id, msg, global_position_int->lat, global_position_int->lon, global_position_int->alt, global_position_int->vx, global_position_int->vy, global_position_int->vz, global_position_int->hdg); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a global_position_int message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 * @param hdg Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, uint16_t hdg) { mavlink_header_t hdr; mavlink_global_position_int_t payload; - uint16_t checksum; - mavlink_global_position_int_t *p = &payload; - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN ) + payload.lat = lat; // int32_t:Latitude, expressed as * 1E7 + payload.lon = lon; // int32_t:Longitude, expressed as * 1E7 + payload.alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters), above MSL + payload.vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + payload.vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + payload.vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + payload.hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xD4, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 831fc6968..8303f350f 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 @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET 49 #define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN 12 #define MAVLINK_MSG_49_LEN 12 +#define MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_KEY 0x3C +#define MAVLINK_MSG_49_KEY 0x3C typedef struct __mavlink_gps_local_origin_set_t { - int32_t latitude; ///< Latitude (WGS84), expressed as * 1E7 - int32_t longitude; ///< Longitude (WGS84), expressed as * 1E7 - int32_t altitude; ///< Altitude(WGS84), expressed as * 1000 + int32_t latitude; ///< Latitude (WGS84), expressed as * 1E7 + int32_t longitude; ///< Longitude (WGS84), expressed as * 1E7 + int32_t altitude; ///< Altitude(WGS84), expressed as * 1000 } mavlink_gps_local_origin_set_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack(uint8_t system_id, mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 - p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 - p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_pack_chan(uint8_t system mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 - p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 - p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id return mavlink_msg_gps_local_origin_set_pack(system_id, component_id, msg, gps_local_origin_set->latitude, gps_local_origin_set->longitude, gps_local_origin_set->altitude); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_local_origin_set message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id * @param longitude Longitude (WGS84), expressed as * 1E7 * @param altitude Altitude(WGS84), expressed as * 1000 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { mavlink_header_t hdr; mavlink_gps_local_origin_set_t payload; - uint16_t checksum; - mavlink_gps_local_origin_set_t *p = &payload; - p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 - p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 - p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN ) + payload.latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 + payload.longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 + payload.altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x3C, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index 0ee5e0f3f..ca4f86e37 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -3,19 +3,21 @@ #define MAVLINK_MSG_ID_GPS_RAW 32 #define MAVLINK_MSG_ID_GPS_RAW_LEN 38 #define MAVLINK_MSG_32_LEN 38 +#define MAVLINK_MSG_ID_GPS_RAW_KEY 0x5B +#define MAVLINK_MSG_32_KEY 0x5B typedef struct __mavlink_gps_raw_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float lat; ///< Latitude in degrees - float lon; ///< Longitude in degrees - float alt; ///< Altitude in meters - float eph; ///< GPS HDOP - float epv; ///< GPS VDOP - float v; ///< GPS ground speed - float hdg; ///< Compass heading in degrees, 0..360 degrees - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - uint8_t satellites_visible; ///< Number of satellites visible + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float lat; ///< Latitude in degrees + float lon; ///< Longitude in degrees + float alt; ///< Altitude in meters + float eph; ///< GPS HDOP + float epv; ///< GPS VDOP + float v; ///< GPS ground speed + float hdg; ///< Compass heading in degrees, 0..360 degrees + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible } mavlink_gps_raw_t; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_gps_raw_pack(uint8_t system_id, uint8_t compo mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // float:Latitude in degrees - p->lon = lon; // float:Longitude in degrees - p->alt = alt; // float:Altitude in meters - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_LEN); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_gps_raw_pack_chan(uint8_t system_id, uint8_t mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // float:Latitude in degrees - p->lon = lon; // float:Longitude in degrees - p->alt = alt; // float:Altitude in meters - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // float:Latitude in degrees + p->lon = lon; // float:Longitude in degrees + p->alt = alt; // float:Altitude in meters + p->eph = eph; // float:GPS HDOP + p->epv = epv; // float:GPS VDOP + p->v = v; // float:GPS ground speed + p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_LEN); } @@ -106,6 +108,8 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com return mavlink_msg_gps_raw_pack(system_id, component_id, msg, gps_raw->usec, gps_raw->fix_type, gps_raw->lat, gps_raw->lon, gps_raw->alt, gps_raw->eph, gps_raw->epv, gps_raw->v, gps_raw->hdg, gps_raw->satellites_visible); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_raw message * @param chan MAVLink channel to send the message @@ -121,26 +125,22 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @param hdg Compass heading in degrees, 0..360 degrees * @param satellites_visible Number of satellites visible */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg, uint8_t satellites_visible) { mavlink_header_t hdr; mavlink_gps_raw_t payload; - uint16_t checksum; - mavlink_gps_raw_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // float:Latitude in degrees - p->lon = lon; // float:Longitude in degrees - p->alt = alt; // float:Altitude in meters - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_RAW_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + payload.lat = lat; // float:Latitude in degrees + payload.lon = lon; // float:Longitude in degrees + payload.alt = alt; // float:Altitude in meters + payload.eph = eph; // float:GPS HDOP + payload.epv = epv; // float:GPS VDOP + payload.v = v; // float:GPS ground speed + payload.hdg = hdg; // float:Compass heading in degrees, 0..360 degrees + payload.satellites_visible = satellites_visible; // uint8_t:Number of satellites visible hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_RAW_LEN; @@ -151,14 +151,12 @@ static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t use mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x5B, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 c0cf3bba9..bc281fe0e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -3,19 +3,21 @@ #define MAVLINK_MSG_ID_GPS_RAW_INT 25 #define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 #define MAVLINK_MSG_25_LEN 30 +#define MAVLINK_MSG_ID_GPS_RAW_INT_KEY 0xA6 +#define MAVLINK_MSG_25_KEY 0xA6 typedef struct __mavlink_gps_raw_int_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int32_t lat; ///< Latitude in 1E7 degrees - int32_t lon; ///< Longitude in 1E7 degrees - int32_t alt; ///< Altitude in 1E3 meters (millimeters) above MSL - uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 - uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int32_t lat; ///< Latitude in 1E7 degrees + int32_t lon; ///< Longitude in 1E7 degrees + int32_t alt; ///< Altitude in 1E3 meters (millimeters) above MSL + uint16_t eph; ///< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t epv; ///< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + uint16_t vel; ///< GPS ground speed (m/s * 100). If unknown, set to: 65535 + uint16_t hdg; ///< Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + uint8_t satellites_visible; ///< Number of satellites visible. If unknown, set to 255 } mavlink_gps_raw_int_t; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // int32_t:Latitude in 1E7 degrees - p->lon = lon; // int32_t:Longitude in 1E7 degrees - p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL - p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // int32_t:Latitude in 1E7 degrees - p->lon = lon; // int32_t:Longitude in 1E7 degrees - p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL - p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + p->lat = lat; // int32_t:Latitude in 1E7 degrees + p->lon = lon; // int32_t:Longitude in 1E7 degrees + p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); } @@ -106,6 +108,8 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->hdg, gps_raw_int->satellites_visible); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_raw_int message * @param chan MAVLink channel to send the message @@ -121,26 +125,22 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @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 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t hdg, uint8_t satellites_visible) { mavlink_header_t hdr; mavlink_gps_raw_int_t payload; - uint16_t checksum; - mavlink_gps_raw_int_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // int32_t:Latitude in 1E7 degrees - p->lon = lon; // int32_t:Longitude in 1E7 degrees - p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL - p->eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 - p->vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 - p->hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_RAW_INT_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + payload.lat = lat; // int32_t:Latitude in 1E7 degrees + payload.lon = lon; // int32_t:Longitude in 1E7 degrees + payload.alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) above MSL + payload.eph = eph; // uint16_t:GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + payload.epv = epv; // uint16_t:GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 + payload.vel = vel; // uint16_t:GPS ground speed (m/s * 100). If unknown, set to: 65535 + payload.hdg = hdg; // uint16_t:Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 + payload.satellites_visible = satellites_visible; // uint8_t:Number of satellites visible. If unknown, set to 255 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; @@ -151,14 +151,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA6, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 bccfdba02..edb2b8d54 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 @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN 48 #define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN 14 #define MAVLINK_MSG_48_LEN 14 +#define MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_KEY 0x8E +#define MAVLINK_MSG_48_KEY 0x8E typedef struct __mavlink_gps_set_global_origin_t { - int32_t latitude; ///< global position * 1E7 - int32_t longitude; ///< global position * 1E7 - int32_t altitude; ///< global position * 1000 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + int32_t latitude; ///< global position * 1E7 + int32_t longitude; ///< global position * 1E7 + int32_t altitude; ///< global position * 1000 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_gps_set_global_origin_t; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->latitude = latitude; // int32_t:global position * 1E7 - p->longitude = longitude; // int32_t:global position * 1E7 - p->altitude = altitude; // int32_t:global position * 1000 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t syste mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->latitude = latitude; // int32_t:global position * 1E7 - p->longitude = longitude; // int32_t:global position * 1E7 - p->altitude = altitude; // int32_t:global position * 1000 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->latitude = latitude; // int32_t:global position * 1E7 + p->longitude = longitude; // int32_t:global position * 1E7 + p->altitude = altitude; // int32_t:global position * 1000 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN); } @@ -81,6 +83,8 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->latitude, gps_set_global_origin->longitude, gps_set_global_origin->altitude); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_set_global_origin message * @param chan MAVLink channel to send the message @@ -91,21 +95,17 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i * @param longitude global position * 1E7 * @param altitude global position * 1000 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { mavlink_header_t hdr; mavlink_gps_set_global_origin_t payload; - uint16_t checksum; - mavlink_gps_set_global_origin_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->latitude = latitude; // int32_t:global position * 1E7 - p->longitude = longitude; // int32_t:global position * 1E7 - p->altitude = altitude; // int32_t:global position * 1000 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.latitude = latitude; // int32_t:global position * 1E7 + payload.longitude = longitude; // int32_t:global position * 1E7 + payload.altitude = altitude; // int32_t:global position * 1000 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; @@ -116,14 +116,12 @@ static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x8E, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index a974432d6..450a98ccd 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_GPS_STATUS 27 #define MAVLINK_MSG_ID_GPS_STATUS_LEN 101 #define MAVLINK_MSG_27_LEN 101 +#define MAVLINK_MSG_ID_GPS_STATUS_KEY 0x63 +#define MAVLINK_MSG_27_KEY 0x63 typedef struct __mavlink_gps_status_t { - uint8_t satellites_visible; ///< Number of satellites visible - uint8_t satellite_prn[20]; ///< Global satellite ID - uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization - uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite - uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. - uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite + uint8_t satellites_visible; ///< Number of satellites visible + uint8_t satellite_prn[20]; ///< Global satellite ID + uint8_t satellite_used[20]; ///< 0: Satellite not used, 1: used for localization + uint8_t satellite_elevation[20]; ///< Elevation (0: right on top of receiver, 90: on the horizon) of satellite + uint8_t satellite_azimuth[20]; ///< Direction of satellite, 0: 0 deg, 255: 360 deg. + uint8_t satellite_snr[20]; ///< Signal to noise ratio of satellite } mavlink_gps_status_t; #define MAVLINK_MSG_GPS_STATUS_FIELD_SATELLITE_PRN_LEN 20 @@ -39,12 +41,12 @@ static inline uint16_t mavlink_msg_gps_status_pack(uint8_t system_id, uint8_t co mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID - memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization - memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite - memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. - memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_STATUS_LEN); } @@ -68,12 +70,12 @@ static inline uint16_t mavlink_msg_gps_status_pack_chan(uint8_t system_id, uint8 mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_STATUS; - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID - memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization - memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite - memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. - memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_STATUS_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t return mavlink_msg_gps_status_pack(system_id, component_id, msg, gps_status->satellites_visible, gps_status->satellite_prn, gps_status->satellite_used, gps_status->satellite_elevation, gps_status->satellite_azimuth, gps_status->satellite_snr); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_status message * @param chan MAVLink channel to send the message @@ -102,22 +106,18 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. * @param satellite_snr Signal to noise ratio of satellite */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { mavlink_header_t hdr; mavlink_gps_status_t payload; - uint16_t checksum; - mavlink_gps_status_t *p = &payload; - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID - memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization - memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite - memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. - memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_STATUS_LEN ) + payload.satellites_visible = satellites_visible; // uint8_t:Number of satellites visible + memcpy(payload.satellite_prn, satellite_prn, sizeof(payload.satellite_prn)); // uint8_t[20]:Global satellite ID + memcpy(payload.satellite_used, satellite_used, sizeof(payload.satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization + memcpy(payload.satellite_elevation, satellite_elevation, sizeof(payload.satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite + memcpy(payload.satellite_azimuth, satellite_azimuth, sizeof(payload.satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. + memcpy(payload.satellite_snr, satellite_snr, sizeof(payload.satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; @@ -128,14 +128,12 @@ static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t s mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x63, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index 7c686831e..466f8a783 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_HEARTBEAT 0 #define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 #define MAVLINK_MSG_0_LEN 3 +#define MAVLINK_MSG_ID_HEARTBEAT_KEY 0x47 +#define MAVLINK_MSG_0_KEY 0x47 typedef struct __mavlink_heartbeat_t { - uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - uint8_t mavlink_version; ///< MAVLink version + uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + uint8_t mavlink_version; ///< MAVLink version } mavlink_heartbeat_t; @@ -27,10 +29,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } @@ -49,10 +51,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } @@ -69,6 +71,8 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -76,20 +80,16 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; mavlink_heartbeat_t payload; - uint16_t checksum; - mavlink_heartbeat_t *p = &payload; - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HEARTBEAT_LEN ) + payload.type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + payload.autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version + payload.mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; @@ -99,14 +99,12 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x47, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index fb6648714..280469009 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_LOCAL_POSITION 31 #define MAVLINK_MSG_ID_LOCAL_POSITION_LEN 32 #define MAVLINK_MSG_31_LEN 32 +#define MAVLINK_MSG_ID_LOCAL_POSITION_KEY 0xF0 +#define MAVLINK_MSG_31_KEY 0xF0 typedef struct __mavlink_local_position_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - float vx; ///< X Speed - float vy; ///< Y Speed - float vz; ///< Z Speed + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + float vx; ///< X Speed + float vy; ///< Y Speed + float vz; ///< Z Speed } mavlink_local_position_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_local_position_pack(uint8_t system_id, uint8_ mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - p->vx = vx; // float:X Speed - p->vy = vy; // float:Y Speed - p->vz = vz; // float:Z Speed + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_local_position_pack_chan(uint8_t system_id, u mavlink_local_position_t *p = (mavlink_local_position_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - p->vx = vx; // float:X Speed - p->vy = vy; // float:Y Speed - p->vz = vz; // float:Z Speed + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + p->vx = vx; // float:X Speed + p->vy = vy; // float:Y Speed + p->vz = vz; // float:Z Speed return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint return mavlink_msg_local_position_pack(system_id, component_id, msg, local_position->usec, local_position->x, local_position->y, local_position->z, local_position->vx, local_position->vy, local_position->vz); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a local_position message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint * @param vy Y Speed * @param vz Z Speed */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { mavlink_header_t hdr; mavlink_local_position_t payload; - uint16_t checksum; - mavlink_local_position_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - p->vx = vx; // float:X Speed - p->vy = vy; // float:Y Speed - p->vz = vz; // float:Z Speed + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LOCAL_POSITION_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.x = x; // float:X Position + payload.y = y; // float:Y Position + payload.z = z; // float:Z Position + payload.vx = vx; // float:X Speed + payload.vy = vy; // float:Y Speed + payload.vz = vz; // float:Z Speed hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint6 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF0, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 8ea4ca458..6fd3bb043 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT 51 #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN 16 #define MAVLINK_MSG_51_LEN 16 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_KEY 0x4B +#define MAVLINK_MSG_51_KEY 0x4B typedef struct __mavlink_local_position_setpoint_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< Desired yaw angle } mavlink_local_position_setpoint_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack(uint8_t system_i mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_local_position_setpoint_pack_chan(uint8_t sys mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system return mavlink_msg_local_position_setpoint_pack(system_id, component_id, msg, local_position_setpoint->x, local_position_setpoint->y, local_position_setpoint->z, local_position_setpoint->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a local_position_setpoint message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system * @param z z position * @param yaw Desired yaw angle */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { mavlink_header_t hdr; mavlink_local_position_setpoint_t payload; - uint16_t checksum; - mavlink_local_position_setpoint_t *p = &payload; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN ) + payload.x = x; // float:x position + payload.y = y; // float:y position + payload.z = z; // float:z position + payload.yaw = yaw; // float:Desired yaw angle hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x4B, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 d7aa0d5bc..c26258c5b 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 @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET 50 #define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN 18 #define MAVLINK_MSG_50_LEN 18 +#define MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_KEY 0xA +#define MAVLINK_MSG_50_KEY 0xA typedef struct __mavlink_local_position_setpoint_set_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< Desired yaw angle - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< Desired yaw angle + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_local_position_setpoint_set_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack(uint8_t syst mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_pack_chan(uint8_t mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:Desired yaw angle return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy return mavlink_msg_local_position_setpoint_set_pack(system_id, component_id, msg, local_position_setpoint_set->target_system, local_position_setpoint_set->target_component, local_position_setpoint_set->x, local_position_setpoint_set->y, local_position_setpoint_set->z, local_position_setpoint_set->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a local_position_setpoint_set message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy * @param z z position * @param yaw Desired yaw angle */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { mavlink_header_t hdr; mavlink_local_position_setpoint_set_t payload; - uint16_t checksum; - mavlink_local_position_setpoint_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.x = x; // float:x position + payload.y = y; // float:y position + payload.z = z; // float:z position + payload.yaw = yaw; // float:Desired yaw angle hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index eeb05487a..a8cecbec5 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_MANUAL_CONTROL 69 #define MAVLINK_MSG_ID_MANUAL_CONTROL_LEN 21 #define MAVLINK_MSG_69_LEN 21 +#define MAVLINK_MSG_ID_MANUAL_CONTROL_KEY 0x7F +#define MAVLINK_MSG_69_KEY 0x7F typedef struct __mavlink_manual_control_t { - float roll; ///< roll - float pitch; ///< pitch - float yaw; ///< yaw - float thrust; ///< thrust - uint8_t target; ///< The system to be controlled - uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 - uint8_t pitch_manual; ///< pitch auto:0, manual:1 - uint8_t yaw_manual; ///< yaw auto:0, manual:1 - uint8_t thrust_manual; ///< thrust auto:0, manual:1 + float roll; ///< roll + float pitch; ///< pitch + float yaw; ///< yaw + float thrust; ///< thrust + uint8_t target; ///< The system to be controlled + uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 + uint8_t pitch_manual; ///< pitch auto:0, manual:1 + uint8_t yaw_manual; ///< yaw auto:0, manual:1 + uint8_t thrust_manual; ///< thrust auto:0, manual:1 } mavlink_manual_control_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_manual_control_pack(uint8_t system_id, uint8_ mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_manual_control_pack_chan(uint8_t system_id, u mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + p->target = target; // uint8_t:The system to be controlled + p->roll = roll; // float:roll + p->pitch = pitch; // float:pitch + p->yaw = yaw; // float:yaw + p->thrust = thrust; // float:thrust + p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint return mavlink_msg_manual_control_pack(system_id, component_id, msg, manual_control->target, manual_control->roll, manual_control->pitch, manual_control->yaw, manual_control->thrust, manual_control->roll_manual, manual_control->pitch_manual, manual_control->yaw_manual, manual_control->thrust_manual); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a manual_control message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint * @param yaw_manual yaw auto:0, manual:1 * @param thrust_manual thrust auto:0, manual:1 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { mavlink_header_t hdr; mavlink_manual_control_t payload; - uint16_t checksum; - mavlink_manual_control_t *p = &payload; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MANUAL_CONTROL_LEN ) + payload.target = target; // uint8_t:The system to be controlled + payload.roll = roll; // float:roll + payload.pitch = pitch; // float:pitch + payload.yaw = yaw; // float:yaw + payload.thrust = thrust; // float:thrust + payload.roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + payload.pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + payload.yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + payload.thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x7F, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h index ce90911d1..22857f9fe 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_memory_vect.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_MEMORY_VECT 250 #define MAVLINK_MSG_ID_MEMORY_VECT_LEN 36 #define MAVLINK_MSG_250_LEN 36 +#define MAVLINK_MSG_ID_MEMORY_VECT_KEY 0x8A +#define MAVLINK_MSG_250_KEY 0x8A typedef struct __mavlink_memory_vect_t { - uint16_t address; ///< Starting address of the debug variables - uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - uint8_t 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 - int8_t value[32]; ///< Memory contents at specified address + uint16_t address; ///< Starting address of the debug variables + uint8_t ver; ///< Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + uint8_t 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 + int8_t value[32]; ///< Memory contents at specified address } mavlink_memory_vect_t; #define MAVLINK_MSG_MEMORY_VECT_FIELD_VALUE_LEN 32 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_memory_vect_pack(uint8_t system_id, uint8_t c mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; - p->address = address; // uint16_t:Starting address of the debug variables - p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - p->type = type; // uint8_t: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 - memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t: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 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MEMORY_VECT_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_memory_vect_pack_chan(uint8_t system_id, uint mavlink_memory_vect_t *p = (mavlink_memory_vect_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MEMORY_VECT; - p->address = address; // uint16_t:Starting address of the debug variables - p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - p->type = type; // uint8_t: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 - memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + p->address = address; // uint16_t:Starting address of the debug variables + p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + p->type = type; // uint8_t: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 + memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN); } @@ -77,6 +79,8 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t return mavlink_msg_memory_vect_pack(system_id, component_id, msg, memory_vect->address, memory_vect->ver, memory_vect->type, memory_vect->value); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a memory_vect message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_memory_vect_encode(uint8_t system_id, uint8_t * @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 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS 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_header_t hdr; mavlink_memory_vect_t payload; - uint16_t checksum; - mavlink_memory_vect_t *p = &payload; - p->address = address; // uint16_t:Starting address of the debug variables - p->ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below - p->type = type; // uint8_t: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 - memcpy(p->value, value, sizeof(p->value)); // int8_t[32]:Memory contents at specified address + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MEMORY_VECT_LEN ) + payload.address = address; // uint16_t:Starting address of the debug variables + payload.ver = ver; // uint8_t:Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below + payload.type = type; // uint8_t: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 + memcpy(payload.value, value, sizeof(payload.value)); // int8_t[32]:Memory contents at specified address hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_MEMORY_VECT_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_memory_vect_send(mavlink_channel_t chan, uint16_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x8A, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 ce5c680ad..6775ea32d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT 252 #define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN 14 #define MAVLINK_MSG_252_LEN 14 +#define MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_KEY 0x8D +#define MAVLINK_MSG_252_KEY 0x8D typedef struct __mavlink_named_value_float_t { - float value; ///< Floating point value - char name[10]; ///< Name of the debug variable + float value; ///< Floating point value + char name[10]; ///< Name of the debug variable } mavlink_named_value_float_t; #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_named_value_float_pack(uint8_t system_id, uin mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_named_value_float_pack_chan(uint8_t system_id mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // float:Floating point value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN); } @@ -67,6 +69,8 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u return mavlink_msg_named_value_float_pack(system_id, component_id, msg, named_value_float->name, named_value_float->value); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a named_value_float message * @param chan MAVLink channel to send the message @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param name Name of the debug variable * @param value Floating point value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) { mavlink_header_t hdr; mavlink_named_value_float_t payload; - uint16_t checksum; - mavlink_named_value_float_t *p = &payload; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN ) + memcpy(payload.name, name, sizeof(payload.name)); // char[10]:Name of the debug variable + payload.value = value; // float:Floating point value hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; @@ -96,14 +96,12 @@ static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, co mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x8D, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 1c75a5be1..86789129e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_NAMED_VALUE_INT 253 #define MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN 14 #define MAVLINK_MSG_253_LEN 14 +#define MAVLINK_MSG_ID_NAMED_VALUE_INT_KEY 0xD3 +#define MAVLINK_MSG_253_KEY 0xD3 typedef struct __mavlink_named_value_int_t { - int32_t value; ///< Signed integer value - char name[10]; ///< Name of the debug variable + int32_t value; ///< Signed integer value + char name[10]; ///< Name of the debug variable } mavlink_named_value_int_t; #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_named_value_int_pack(uint8_t system_id, uint8 mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_named_value_int_pack_chan(uint8_t system_id, mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value + memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable + p->value = value; // int32_t:Signed integer value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN); } @@ -67,6 +69,8 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin return mavlink_msg_named_value_int_pack(system_id, component_id, msg, named_value_int->name, named_value_int->value); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a named_value_int message * @param chan MAVLink channel to send the message @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param name Name of the debug variable * @param value Signed integer value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) { mavlink_header_t hdr; mavlink_named_value_int_t payload; - uint16_t checksum; - mavlink_named_value_int_t *p = &payload; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN ) + memcpy(payload.name, name, sizeof(payload.name)); // char[10]:Name of the debug variable + payload.value = value; // int32_t:Signed integer value hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; @@ -96,14 +96,12 @@ static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, cons mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xD3, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 ef878a3f7..351fa5205 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT 62 #define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN 26 #define MAVLINK_MSG_62_LEN 26 +#define MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_KEY 0x3E +#define MAVLINK_MSG_62_KEY 0x3E typedef struct __mavlink_nav_controller_output_t { - float nav_roll; ///< Current desired roll in degrees - float nav_pitch; ///< Current desired pitch in degrees - float alt_error; ///< Current altitude error in meters - float aspd_error; ///< Current airspeed error in meters/second - float xtrack_error; ///< Current crosstrack error on x-y plane in meters - int16_t nav_bearing; ///< Current desired heading in degrees - int16_t target_bearing; ///< Bearing to current waypoint/target in degrees - uint16_t wp_dist; ///< Distance to active waypoint in meters + float nav_roll; ///< Current desired roll in degrees + float nav_pitch; ///< Current desired pitch in degrees + float alt_error; ///< Current altitude error in meters + float aspd_error; ///< Current airspeed error in meters/second + float xtrack_error; ///< Current crosstrack error on x-y plane in meters + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current waypoint/target in degrees + uint16_t wp_dist; ///< Distance to active waypoint in meters } mavlink_nav_controller_output_t; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack(uint8_t system_id, mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - p->nav_roll = nav_roll; // float:Current desired roll in degrees - p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees - p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees - p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees - p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters - p->alt_error = alt_error; // float:Current altitude error in meters - p->aspd_error = aspd_error; // float:Current airspeed error in meters/second - p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_nav_controller_output_pack_chan(uint8_t syste mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - p->nav_roll = nav_roll; // float:Current desired roll in degrees - p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees - p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees - p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees - p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters - p->alt_error = alt_error; // float:Current altitude error in meters - p->aspd_error = aspd_error; // float:Current airspeed error in meters/second - p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + p->nav_roll = nav_roll; // float:Current desired roll in degrees + p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees + p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + p->alt_error = alt_error; // float:Current altitude error in meters + p->aspd_error = aspd_error; // float:Current airspeed error in meters/second + p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN); } @@ -96,6 +98,8 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i return mavlink_msg_nav_controller_output_pack(system_id, component_id, msg, nav_controller_output->nav_roll, nav_controller_output->nav_pitch, nav_controller_output->nav_bearing, nav_controller_output->target_bearing, nav_controller_output->wp_dist, nav_controller_output->alt_error, nav_controller_output->aspd_error, nav_controller_output->xtrack_error); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a nav_controller_output message * @param chan MAVLink channel to send the message @@ -109,24 +113,20 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i * @param aspd_error Current airspeed error in meters/second * @param xtrack_error Current crosstrack error on x-y plane in meters */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { mavlink_header_t hdr; mavlink_nav_controller_output_t payload; - uint16_t checksum; - mavlink_nav_controller_output_t *p = &payload; - - p->nav_roll = nav_roll; // float:Current desired roll in degrees - p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees - p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees - p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees - p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters - p->alt_error = alt_error; // float:Current altitude error in meters - p->aspd_error = aspd_error; // float:Current airspeed error in meters/second - p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN ) + payload.nav_roll = nav_roll; // float:Current desired roll in degrees + payload.nav_pitch = nav_pitch; // float:Current desired pitch in degrees + payload.nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees + payload.target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees + payload.wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters + payload.alt_error = alt_error; // float:Current altitude error in meters + payload.aspd_error = aspd_error; // float:Current airspeed error in meters/second + payload.xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; @@ -137,14 +137,12 @@ static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x3E, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 74ed51442..28fb9c414 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST 21 #define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN 2 #define MAVLINK_MSG_21_LEN 2 +#define MAVLINK_MSG_ID_PARAM_REQUEST_LIST_KEY 0x22 +#define MAVLINK_MSG_21_KEY 0x22 typedef struct __mavlink_param_request_list_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_param_request_list_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_param_request_list_pack(uint8_t system_id, ui mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_param_request_list_pack_chan(uint8_t system_i mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, return mavlink_msg_param_request_list_pack(system_id, component_id, msg, param_request_list->target_system, param_request_list->target_component); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_request_list message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, * @param target_system System ID * @param target_component Component ID */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { mavlink_header_t hdr; mavlink_param_request_list_t payload; - uint16_t checksum; - mavlink_param_request_list_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x22, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 86c0a4b1d..614fc4328 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 #define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 #define MAVLINK_MSG_20_LEN 20 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_KEY 0x21 +#define MAVLINK_MSG_20_KEY 0x21 typedef struct __mavlink_param_request_read_t { - int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - char param_id[16]; ///< Onboard parameter id + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + char param_id[16]; ///< Onboard parameter id } mavlink_param_request_read_t; #define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_i mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); } @@ -77,6 +79,8 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, return mavlink_msg_param_request_read_pack(system_id, component_id, msg, param_request_read->target_system, param_request_read->target_component, param_request_read->param_id, param_request_read->param_index); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_request_read message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_id Onboard parameter id * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) { mavlink_header_t hdr; mavlink_param_request_read_t payload; - uint16_t checksum; - mavlink_param_request_read_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + memcpy(payload.param_id, param_id, sizeof(payload.param_id)); // char[16]:Onboard parameter id + payload.param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x21, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index 7dfb5b6dc..79e508be3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PARAM_SET 23 #define MAVLINK_MSG_ID_PARAM_SET_LEN 22 #define MAVLINK_MSG_23_LEN 22 +#define MAVLINK_MSG_ID_PARAM_SET_KEY 0x2D +#define MAVLINK_MSG_23_KEY 0x2D typedef struct __mavlink_param_set_t { - float param_value; ///< Onboard parameter value - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - char param_id[16]; ///< Onboard parameter id + float param_value; ///< Onboard parameter value + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + char param_id[16]; ///< Onboard parameter id } mavlink_param_set_t; #define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_ mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); } @@ -77,6 +79,8 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c return mavlink_msg_param_set_pack(system_id, component_id, msg, param_set->target_system, param_set->target_component, param_set->param_id, param_set->param_value); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_set message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_id Onboard parameter id * @param param_value Onboard parameter value */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) { mavlink_header_t hdr; mavlink_param_set_t payload; - uint16_t checksum; - mavlink_param_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_SET_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + memcpy(payload.param_id, param_id, sizeof(payload.param_id)); // char[16]:Onboard parameter id + payload.param_value = param_value; // float:Onboard parameter value hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PARAM_SET_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x2D, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 19f618ab4..877a90484 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PARAM_VALUE 22 #define MAVLINK_MSG_ID_PARAM_VALUE_LEN 24 #define MAVLINK_MSG_22_LEN 24 +#define MAVLINK_MSG_ID_PARAM_VALUE_KEY 0xA3 +#define MAVLINK_MSG_22_KEY 0xA3 typedef struct __mavlink_param_value_t { - float param_value; ///< Onboard parameter value - uint16_t param_count; ///< Total number of onboard parameters - uint16_t param_index; ///< Index of this onboard parameter - char param_id[16]; ///< Onboard parameter id + float param_value; ///< Onboard parameter value + uint16_t param_count; ///< Total number of onboard parameters + uint16_t param_index; ///< Index of this onboard parameter + char param_id[16]; ///< Onboard parameter id } mavlink_param_value_t; #define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id + p->param_value = param_value; // float:Onboard parameter value + p->param_count = param_count; // uint16_t:Total number of onboard parameters + p->param_index = param_index; // uint16_t:Index of this onboard parameter return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN); } @@ -77,6 +79,8 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t return mavlink_msg_param_value_pack(system_id, component_id, msg, param_value->param_id, param_value->param_value, param_value->param_count, param_value->param_index); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a param_value message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_count Total number of onboard parameters * @param param_index Index of this onboard parameter */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_header_t hdr; mavlink_param_value_t payload; - uint16_t checksum; - mavlink_param_value_t *p = &payload; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[16]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PARAM_VALUE_LEN ) + memcpy(payload.param_id, param_id, sizeof(payload.param_id)); // char[16]:Onboard parameter id + payload.param_value = param_value; // float:Onboard parameter value + payload.param_count = param_count; // uint16_t:Total number of onboard parameters + payload.param_index = param_index; // uint16_t:Index of this onboard parameter hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA3, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index f22a195c7..7cf69d57c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PING 3 #define MAVLINK_MSG_ID_PING_LEN 14 #define MAVLINK_MSG_3_LEN 14 +#define MAVLINK_MSG_ID_PING_KEY 0xE2 +#define MAVLINK_MSG_3_KEY 0xE2 typedef struct __mavlink_ping_t { - uint64_t time; ///< Unix timestamp in microseconds - uint32_t seq; ///< PING sequence - uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + uint64_t time; ///< Unix timestamp in microseconds + uint32_t seq; ///< PING sequence + uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system } mavlink_ping_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_ping_pack(uint8_t system_id, uint8_t componen mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - p->seq = seq; // uint32_t:PING sequence - p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - p->time = time; // uint64_t:Unix timestamp in microseconds + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PING_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_ping_pack_chan(uint8_t system_id, uint8_t com mavlink_ping_t *p = (mavlink_ping_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PING; - p->seq = seq; // uint32_t:PING sequence - p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - p->time = time; // uint64_t:Unix timestamp in microseconds + p->seq = seq; // uint32_t:PING sequence + p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + p->time = time; // uint64_t:Unix timestamp in microseconds return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PING_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon return mavlink_msg_ping_pack(system_id, component_id, msg, ping->seq, ping->target_system, ping->target_component, ping->time); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a ping message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system * @param time Unix timestamp in microseconds */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { mavlink_header_t hdr; mavlink_ping_t payload; - uint16_t checksum; - mavlink_ping_t *p = &payload; - p->seq = seq; // uint32_t:PING sequence - p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - p->time = time; // uint64_t:Unix timestamp in microseconds + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PING_LEN ) + payload.seq = seq; // uint32_t:PING sequence + payload.target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + payload.target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + payload.time = time; // uint64_t:Unix timestamp in microseconds hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PING_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xE2, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index 1a65ed0a4..113055c3d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_POSITION_TARGET 63 #define MAVLINK_MSG_ID_POSITION_TARGET_LEN 16 #define MAVLINK_MSG_63_LEN 16 +#define MAVLINK_MSG_ID_POSITION_TARGET_KEY 0x4B +#define MAVLINK_MSG_63_KEY 0x4B typedef struct __mavlink_position_target_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< yaw orientation in radians, 0 = NORTH } mavlink_position_target_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_position_target_pack(uint8_t system_id, uint8 mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_position_target_pack_chan(uint8_t system_id, mavlink_position_target_t *p = (mavlink_position_target_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_TARGET; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin return mavlink_msg_position_target_pack(system_id, component_id, msg, position_target->x, position_target->y, position_target->z, position_target->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_target message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { mavlink_header_t hdr; mavlink_position_target_t payload; - uint16_t checksum; - mavlink_position_target_t *p = &payload; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_TARGET_LEN ) + payload.x = x; // float:x position + payload.y = y; // float:y position + payload.z = z; // float:z position + payload.yaw = yaw; // float:yaw orientation in radians, 0 = NORTH hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, floa mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x4B, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index aa0f30672..ad3acf058 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -3,19 +3,21 @@ #define MAVLINK_MSG_ID_RAW_IMU 28 #define MAVLINK_MSG_ID_RAW_IMU_LEN 26 #define MAVLINK_MSG_28_LEN 26 +#define MAVLINK_MSG_ID_RAW_IMU_KEY 0x1C +#define MAVLINK_MSG_28_KEY 0x1C typedef struct __mavlink_raw_imu_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t xacc; ///< X acceleration (raw) - int16_t yacc; ///< Y acceleration (raw) - int16_t zacc; ///< Z acceleration (raw) - int16_t xgyro; ///< Angular speed around X axis (raw) - int16_t ygyro; ///< Angular speed around Y axis (raw) - int16_t zgyro; ///< Angular speed around Z axis (raw) - int16_t xmag; ///< X Magnetic field (raw) - int16_t ymag; ///< Y Magnetic field (raw) - int16_t zmag; ///< Z Magnetic field (raw) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (raw) + int16_t yacc; ///< Y acceleration (raw) + int16_t zacc; ///< Z acceleration (raw) + int16_t xgyro; ///< Angular speed around X axis (raw) + int16_t ygyro; ///< Angular speed around Y axis (raw) + int16_t zgyro; ///< Angular speed around Z axis (raw) + int16_t xmag; ///< X Magnetic field (raw) + int16_t ymag; ///< Y Magnetic field (raw) + int16_t zmag; ///< Z Magnetic field (raw) } mavlink_raw_imu_t; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (raw) - p->yacc = yacc; // int16_t:Y acceleration (raw) - p->zacc = zacc; // int16_t:Z acceleration (raw) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) - p->xmag = xmag; // int16_t:X Magnetic field (raw) - p->ymag = ymag; // int16_t:Y Magnetic field (raw) - p->zmag = zmag; // int16_t:Z Magnetic field (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_IMU_LEN); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_IMU; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (raw) - p->yacc = yacc; // int16_t:Y acceleration (raw) - p->zacc = zacc; // int16_t:Z acceleration (raw) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) - p->xmag = xmag; // int16_t:X Magnetic field (raw) - p->ymag = ymag; // int16_t:Y Magnetic field (raw) - p->zmag = zmag; // int16_t:Z Magnetic field (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (raw) + p->yacc = yacc; // int16_t:Y acceleration (raw) + p->zacc = zacc; // int16_t:Z acceleration (raw) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + p->xmag = xmag; // int16_t:X Magnetic field (raw) + p->ymag = ymag; // int16_t:Y Magnetic field (raw) + p->zmag = zmag; // int16_t:Z Magnetic field (raw) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_IMU_LEN); } @@ -106,6 +108,8 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a raw_imu message * @param chan MAVLink channel to send the message @@ -121,26 +125,22 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { mavlink_header_t hdr; mavlink_raw_imu_t payload; - uint16_t checksum; - mavlink_raw_imu_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (raw) - p->yacc = yacc; // int16_t:Y acceleration (raw) - p->zacc = zacc; // int16_t:Z acceleration (raw) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) - p->xmag = xmag; // int16_t:X Magnetic field (raw) - p->ymag = ymag; // int16_t:Y Magnetic field (raw) - p->zmag = zmag; // int16_t:Z Magnetic field (raw) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RAW_IMU_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.xacc = xacc; // int16_t:X acceleration (raw) + payload.yacc = yacc; // int16_t:Y acceleration (raw) + payload.zacc = zacc; // int16_t:Z acceleration (raw) + payload.xgyro = xgyro; // int16_t:Angular speed around X axis (raw) + payload.ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) + payload.zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) + payload.xmag = xmag; // int16_t:X Magnetic field (raw) + payload.ymag = ymag; // int16_t:Y Magnetic field (raw) + payload.zmag = zmag; // int16_t:Z Magnetic field (raw) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RAW_IMU_LEN; @@ -151,14 +151,12 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t use mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x1C, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index 4d3a9ba25..3ae58d82c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_RAW_PRESSURE 29 #define MAVLINK_MSG_ID_RAW_PRESSURE_LEN 16 #define MAVLINK_MSG_29_LEN 16 +#define MAVLINK_MSG_ID_RAW_PRESSURE_KEY 0x15 +#define MAVLINK_MSG_29_KEY 0x15 typedef struct __mavlink_raw_pressure_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t press_abs; ///< Absolute pressure (raw) - int16_t press_diff1; ///< Differential pressure 1 (raw) - int16_t press_diff2; ///< Differential pressure 2 (raw) - int16_t temperature; ///< Raw Temperature measurement (raw) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t press_abs; ///< Absolute pressure (raw) + int16_t press_diff1; ///< Differential pressure 1 (raw) + int16_t press_diff2; ///< Differential pressure 2 (raw) + int16_t temperature; ///< Raw Temperature measurement (raw) } mavlink_raw_pressure_t; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_raw_pressure_pack(uint8_t system_id, uint8_t mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // int16_t:Absolute pressure (raw) - p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) - p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) - p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_raw_pressure_pack_chan(uint8_t system_id, uin mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // int16_t:Absolute pressure (raw) - p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) - p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) - p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // int16_t:Absolute pressure (raw) + p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN); } @@ -81,6 +83,8 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ return mavlink_msg_raw_pressure_pack(system_id, component_id, msg, raw_pressure->usec, raw_pressure->press_abs, raw_pressure->press_diff1, raw_pressure->press_diff2, raw_pressure->temperature); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a raw_pressure message * @param chan MAVLink channel to send the message @@ -91,21 +95,17 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ * @param press_diff2 Differential pressure 2 (raw) * @param temperature Raw Temperature measurement (raw) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { mavlink_header_t hdr; mavlink_raw_pressure_t payload; - uint16_t checksum; - mavlink_raw_pressure_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // int16_t:Absolute pressure (raw) - p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) - p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) - p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RAW_PRESSURE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.press_abs = press_abs; // int16_t:Absolute pressure (raw) + payload.press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) + payload.press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) + payload.temperature = temperature; // int16_t:Raw Temperature measurement (raw) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; @@ -116,14 +116,12 @@ static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_ mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x15, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 b9dfe85e9..1ec726f9d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -3,19 +3,21 @@ #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 #define MAVLINK_MSG_70_LEN 18 +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_KEY 0xC8 +#define MAVLINK_MSG_70_KEY 0xC8 typedef struct __mavlink_rc_channels_override_t { - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_rc_channels_override_t; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); } @@ -106,6 +108,8 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a rc_channels_override message * @param chan MAVLink channel to send the message @@ -121,26 +125,22 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id * @param chan7_raw RC channel 7 value, in microseconds * @param chan8_raw RC channel 8 value, in microseconds */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) { mavlink_header_t hdr; mavlink_rc_channels_override_t payload; - uint16_t checksum; - mavlink_rc_channels_override_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + payload.chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + payload.chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + payload.chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + payload.chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + payload.chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + payload.chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + payload.chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN; @@ -151,14 +151,12 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xC8, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 56de4d83c..840cfbf2c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_RC_CHANNELS_RAW 35 #define MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN 17 #define MAVLINK_MSG_35_LEN 17 +#define MAVLINK_MSG_ID_RC_CHANNELS_RAW_KEY 0x2B +#define MAVLINK_MSG_35_KEY 0x2B typedef struct __mavlink_rc_channels_raw_t { - uint16_t chan1_raw; ///< RC channel 1 value, in microseconds - uint16_t chan2_raw; ///< RC channel 2 value, in microseconds - uint16_t chan3_raw; ///< RC channel 3 value, in microseconds - uint16_t chan4_raw; ///< RC channel 4 value, in microseconds - uint16_t chan5_raw; ///< RC channel 5 value, in microseconds - uint16_t chan6_raw; ///< RC channel 6 value, in microseconds - uint16_t chan7_raw; ///< RC channel 7 value, in microseconds - uint16_t chan8_raw; ///< RC channel 8 value, in microseconds - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% } mavlink_rc_channels_raw_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack(uint8_t system_id, uint8 mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_rc_channels_raw_pack_chan(uint8_t system_id, mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin return mavlink_msg_rc_channels_raw_pack(system_id, component_id, msg, rc_channels_raw->chan1_raw, rc_channels_raw->chan2_raw, rc_channels_raw->chan3_raw, rc_channels_raw->chan4_raw, rc_channels_raw->chan5_raw, rc_channels_raw->chan6_raw, rc_channels_raw->chan7_raw, rc_channels_raw->chan8_raw, rc_channels_raw->rssi); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a rc_channels_raw message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin * @param chan8_raw RC channel 8 value, in microseconds * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) { mavlink_header_t hdr; mavlink_rc_channels_raw_t payload; - uint16_t checksum; - mavlink_rc_channels_raw_t *p = &payload; - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN ) + payload.chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + payload.chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + payload.chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + payload.chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + payload.chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + payload.chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + payload.chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + payload.chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + payload.rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x2B, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 fd2e57ad4..95ba39510 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED 36 #define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN 17 #define MAVLINK_MSG_36_LEN 17 +#define MAVLINK_MSG_ID_RC_CHANNELS_SCALED_KEY 0xC0 +#define MAVLINK_MSG_36_KEY 0xC0 typedef struct __mavlink_rc_channels_scaled_t { - int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% + int16_t chan1_scaled; ///< RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan2_scaled; ///< RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan3_scaled; ///< RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan4_scaled; ///< RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan5_scaled; ///< RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan6_scaled; ///< RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan7_scaled; ///< RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + int16_t chan8_scaled; ///< RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + uint8_t rssi; ///< Receive signal strength indicator, 0: 0%, 255: 100% } mavlink_rc_channels_scaled_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack(uint8_t system_id, ui mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_pack_chan(uint8_t system_i mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, return mavlink_msg_rc_channels_scaled_pack(system_id, component_id, msg, rc_channels_scaled->chan1_scaled, rc_channels_scaled->chan2_scaled, rc_channels_scaled->chan3_scaled, rc_channels_scaled->chan4_scaled, rc_channels_scaled->chan5_scaled, rc_channels_scaled->chan6_scaled, rc_channels_scaled->chan7_scaled, rc_channels_scaled->chan8_scaled, rc_channels_scaled->rssi); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a rc_channels_scaled message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { mavlink_header_t hdr; mavlink_rc_channels_scaled_t payload; - uint16_t checksum; - mavlink_rc_channels_scaled_t *p = &payload; - p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN ) + payload.chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + payload.rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, i mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xC0, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 b779e267e..0c6dd6562 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_REQUEST_DATA_STREAM 66 #define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN 6 #define MAVLINK_MSG_66_LEN 6 +#define MAVLINK_MSG_ID_REQUEST_DATA_STREAM_KEY 0x2A +#define MAVLINK_MSG_66_KEY 0x2A typedef struct __mavlink_request_data_stream_t { - uint16_t req_message_rate; ///< The requested interval between two messages of this type - uint8_t target_system; ///< The target requested to send the message stream. - uint8_t target_component; ///< The target requested to send the message stream. - uint8_t req_stream_id; ///< The ID of the requested message type - uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. + uint16_t req_message_rate; ///< The requested interval between two messages of this type + uint8_t target_system; ///< The target requested to send the message stream. + uint8_t target_component; ///< The target requested to send the message stream. + uint8_t req_stream_id; ///< The ID of the requested message type + uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - p->target_system = target_system; // uint8_t:The target requested to send the message stream. - p->target_component = target_component; // uint8_t:The target requested to send the message stream. - p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - p->target_system = target_system; // uint8_t:The target requested to send the message stream. - p->target_component = target_component; // uint8_t:The target requested to send the message stream. - p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + p->target_system = target_system; // uint8_t:The target requested to send the message stream. + p->target_component = target_component; // uint8_t:The target requested to send the message stream. + p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN); } @@ -81,6 +83,8 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, return mavlink_msg_request_data_stream_pack(system_id, component_id, msg, request_data_stream->target_system, request_data_stream->target_component, request_data_stream->req_stream_id, request_data_stream->req_message_rate, request_data_stream->start_stop); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a request_data_stream message * @param chan MAVLink channel to send the message @@ -91,21 +95,17 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @param req_message_rate The requested interval between two messages of this type * @param start_stop 1 to start sending, 0 to stop sending. */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { mavlink_header_t hdr; mavlink_request_data_stream_t payload; - uint16_t checksum; - mavlink_request_data_stream_t *p = &payload; - p->target_system = target_system; // uint8_t:The target requested to send the message stream. - p->target_component = target_component; // uint8_t:The target requested to send the message stream. - p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN ) + payload.target_system = target_system; // uint8_t:The target requested to send the message stream. + payload.target_component = target_component; // uint8_t:The target requested to send the message stream. + payload.req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type + payload.req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type + payload.start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; @@ -116,14 +116,12 @@ static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x2A, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 new file mode 100644 index 000000000..a9f7231f4 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h @@ -0,0 +1,194 @@ +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 58 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN 20 +#define MAVLINK_MSG_58_LEN 20 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_KEY 0xF5 +#define MAVLINK_MSG_58_KEY 0xF5 + +typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t +{ + uint32_t time_ms; ///< Timestamp in milliseconds since system boot + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 + +} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t; + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_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 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param 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 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_ms, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust); +} + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_ms Timestamp in milliseconds since system boot + * @param 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_send(mavlink_channel_t chan, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_header_t hdr; + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t payload; + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN ) + payload.time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + payload.roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + payload.pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + payload.yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF5, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END +} + +#endif +// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_ms(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + return (uint32_t)(p->time_ms); +} + +/** + * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint) +{ + memcpy( roll_pitch_yaw_speed_thrust_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_speed_thrust_setpoint_t)); +} 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 new file mode 100644 index 000000000..fb00bec61 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h @@ -0,0 +1,194 @@ +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING + +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 57 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN 20 +#define MAVLINK_MSG_57_LEN 20 +#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_KEY 0x38 +#define MAVLINK_MSG_57_KEY 0x38 + +typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t +{ + uint32_t time_ms; ///< Timestamp in milliseconds since system boot + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 + +} mavlink_roll_pitch_yaw_thrust_setpoint_t; + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_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 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +} + +/** + * @brief Pack a roll_pitch_yaw_thrust_setpoint message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param 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 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + + p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN); +} + +/** + * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_ms, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust); +} + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief Send a roll_pitch_yaw_thrust_setpoint message + * @param chan MAVLink channel to send the message + * + * @param time_ms Timestamp in milliseconds since system boot + * @param 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_send(mavlink_channel_t chan, uint32_t time_ms, float roll, float pitch, float yaw, float thrust) +{ + mavlink_header_t hdr; + mavlink_roll_pitch_yaw_thrust_setpoint_t payload; + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN ) + payload.time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot + payload.roll = roll; // float:Desired roll angle in radians + payload.pitch = pitch; // float:Desired pitch angle in radians + payload.yaw = yaw; // float:Desired yaw angle in radians + payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN; + hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x38, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END +} + +#endif +// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING + +/** + * @brief Get field time_ms from roll_pitch_yaw_thrust_setpoint message + * + * @return Timestamp in milliseconds since system boot + */ +static inline uint32_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_ms(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (uint32_t)(p->time_ms); +} + +/** + * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg) +{ + mavlink_roll_pitch_yaw_thrust_setpoint_t *p = (mavlink_roll_pitch_yaw_thrust_setpoint_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct + * + * @param msg The message to decode + * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into + */ +static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint) +{ + memcpy( roll_pitch_yaw_thrust_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_thrust_setpoint_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h index a660e8bed..899d12bcb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA 54 #define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN 25 #define MAVLINK_MSG_54_LEN 25 +#define MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_KEY 0xEA +#define MAVLINK_MSG_54_KEY 0xEA typedef struct __mavlink_safety_allowed_area_t { - float p1x; ///< x position 1 / Latitude 1 - float p1y; ///< y position 1 / Longitude 1 - float p1z; ///< z position 1 / Altitude 1 - float p2x; ///< x position 2 / Latitude 2 - float p2y; ///< y position 2 / Longitude 2 - float p2z; ///< z position 2 / Altitude 2 - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. } mavlink_safety_allowed_area_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack(uint8_t system_id, u mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_safety_allowed_area_pack_chan(uint8_t system_ mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, return mavlink_msg_safety_allowed_area_pack(system_id, component_id, msg, safety_allowed_area->frame, safety_allowed_area->p1x, safety_allowed_area->p1y, safety_allowed_area->p1z, safety_allowed_area->p2x, safety_allowed_area->p2y, safety_allowed_area->p2z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a safety_allowed_area message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, * @param p2y y position 2 / Longitude 2 * @param p2z z position 2 / Altitude 2 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { mavlink_header_t hdr; mavlink_safety_allowed_area_t payload; - uint16_t checksum; - mavlink_safety_allowed_area_t *p = &payload; - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN ) + payload.frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + payload.p1x = p1x; // float:x position 1 / Latitude 1 + payload.p1y = p1y; // float:y position 1 / Longitude 1 + payload.p1z = p1z; // float:z position 1 / Altitude 1 + payload.p2x = p2x; // float:x position 2 / Latitude 2 + payload.p2y = p2y; // float:y position 2 / Longitude 2 + payload.p2z = p2z; // float:z position 2 / Altitude 2 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xEA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 9e8ef1e44..b7afcfd28 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 @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA 53 #define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN 27 #define MAVLINK_MSG_53_LEN 27 +#define MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_KEY 0xF7 +#define MAVLINK_MSG_53_KEY 0xF7 typedef struct __mavlink_safety_set_allowed_area_t { - float p1x; ///< x position 1 / Latitude 1 - float p1y; ///< y position 1 / Longitude 1 - float p1z; ///< z position 1 / Altitude 1 - float p2x; ///< x position 2 / Latitude 2 - float p2y; ///< y position 2 / Longitude 2 - float p2z; ///< z position 2 / Altitude 2 - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + float p1x; ///< x position 1 / Latitude 1 + float p1y; ///< y position 1 / Longitude 1 + float p1z; ///< z position 1 / Altitude 1 + float p2x; ///< x position 2 / Latitude 2 + float p2y; ///< y position 2 / Longitude 2 + float p2z; ///< z position 2 / Altitude 2 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. } mavlink_safety_set_allowed_area_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack(uint8_t system_i mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_pack_chan(uint8_t sys mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + p->p1x = p1x; // float:x position 1 / Latitude 1 + p->p1y = p1y; // float:y position 1 / Longitude 1 + p->p1z = p1z; // float:z position 1 / Altitude 1 + p->p2x = p2x; // float:x position 2 / Latitude 2 + p->p2y = p2y; // float:y position 2 / Longitude 2 + p->p2z = p2z; // float:z position 2 / Altitude 2 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system return mavlink_msg_safety_set_allowed_area_pack(system_id, component_id, msg, safety_set_allowed_area->target_system, safety_set_allowed_area->target_component, safety_set_allowed_area->frame, safety_set_allowed_area->p1x, safety_set_allowed_area->p1y, safety_set_allowed_area->p1z, safety_set_allowed_area->p2x, safety_set_allowed_area->p2y, safety_set_allowed_area->p2z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a safety_set_allowed_area message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system * @param p2y y position 2 / Longitude 2 * @param p2z z position 2 / Altitude 2 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { mavlink_header_t hdr; mavlink_safety_set_allowed_area_t payload; - uint16_t checksum; - mavlink_safety_set_allowed_area_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + payload.p1x = p1x; // float:x position 1 / Latitude 1 + payload.p1y = p1y; // float:y position 1 / Longitude 1 + payload.p1z = p1z; // float:z position 1 / Altitude 1 + payload.p2x = p2x; // float:x position 2 / Latitude 2 + payload.p2y = p2y; // float:y position 2 / Longitude 2 + payload.p2z = p2z; // float:z position 2 / Altitude 2 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t ch mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF7, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 8f381f22c..e6450010b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -3,19 +3,21 @@ #define MAVLINK_MSG_ID_SCALED_IMU 26 #define MAVLINK_MSG_ID_SCALED_IMU_LEN 26 #define MAVLINK_MSG_26_LEN 26 +#define MAVLINK_MSG_ID_SCALED_IMU_KEY 0x1C +#define MAVLINK_MSG_26_KEY 0x1C typedef struct __mavlink_scaled_imu_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - int16_t xacc; ///< X acceleration (mg) - int16_t yacc; ///< Y acceleration (mg) - int16_t zacc; ///< Z acceleration (mg) - int16_t xgyro; ///< Angular speed around X axis (millirad /sec) - int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) - int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) - int16_t xmag; ///< X Magnetic field (milli tesla) - int16_t ymag; ///< Y Magnetic field (milli tesla) - int16_t zmag; ///< Z Magnetic field (milli tesla) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + int16_t xgyro; ///< Angular speed around X axis (millirad /sec) + int16_t ygyro; ///< Angular speed around Y axis (millirad /sec) + int16_t zgyro; ///< Angular speed around Z axis (millirad /sec) + int16_t xmag; ///< X Magnetic field (milli tesla) + int16_t ymag; ///< Y Magnetic field (milli tesla) + int16_t zmag; ///< Z Magnetic field (milli tesla) } mavlink_scaled_imu_t; @@ -42,16 +44,16 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) - p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) - p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) - p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_IMU_LEN); } @@ -79,16 +81,16 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_IMU; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) - p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) - p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) - p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) + p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_IMU_LEN); } @@ -106,6 +108,8 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->usec, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a scaled_imu message * @param chan MAVLink channel to send the message @@ -121,26 +125,22 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t * @param ymag Y Magnetic field (milli tesla) * @param zmag Z Magnetic field (milli tesla) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { mavlink_header_t hdr; mavlink_scaled_imu_t payload; - uint16_t checksum; - mavlink_scaled_imu_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) - p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) - p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) - p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SCALED_IMU_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.xacc = xacc; // int16_t:X acceleration (mg) + payload.yacc = yacc; // int16_t:Y acceleration (mg) + payload.zacc = zacc; // int16_t:Z acceleration (mg) + payload.xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) + payload.ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) + payload.zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) + payload.xmag = xmag; // int16_t:X Magnetic field (milli tesla) + payload.ymag = ymag; // int16_t:Y Magnetic field (milli tesla) + payload.zmag = zmag; // int16_t:Z Magnetic field (milli tesla) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; @@ -151,14 +151,12 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x1C, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index 39d29440f..59b1dcc62 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_SCALED_PRESSURE 38 #define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 18 #define MAVLINK_MSG_38_LEN 18 +#define MAVLINK_MSG_ID_SCALED_PRESSURE_KEY 0x16 +#define MAVLINK_MSG_38_KEY 0x16 typedef struct __mavlink_scaled_pressure_t { - uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - float press_abs; ///< Absolute pressure (hectopascal) - float press_diff; ///< Differential pressure 1 (hectopascal) - int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float press_abs; ///< Absolute pressure (hectopascal) + float press_diff; ///< Differential pressure 1 (hectopascal) + int16_t temperature; ///< Temperature measurement (0.01 degrees celsius) } mavlink_scaled_pressure_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // float:Absolute pressure (hectopascal) - p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) - p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // float:Absolute pressure (hectopascal) - p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) - p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->press_abs = press_abs; // float:Absolute pressure (hectopascal) + p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->usec, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a scaled_pressure message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin * @param press_diff Differential pressure 1 (hectopascal) * @param temperature Temperature measurement (0.01 degrees celsius) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { mavlink_header_t hdr; mavlink_scaled_pressure_t payload; - uint16_t checksum; - mavlink_scaled_pressure_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // float:Absolute pressure (hectopascal) - p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) - p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + payload.press_abs = press_abs; // float:Absolute pressure (hectopascal) + payload.press_diff = press_diff; // float:Differential pressure 1 (hectopascal) + payload.temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x16, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 5aa3cc8d0..976506d56 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 37 #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 16 #define MAVLINK_MSG_37_LEN 16 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_KEY 0xEA +#define MAVLINK_MSG_37_KEY 0xEA typedef struct __mavlink_servo_output_raw_t { - uint16_t servo1_raw; ///< Servo output 1 value, in microseconds - uint16_t servo2_raw; ///< Servo output 2 value, in microseconds - uint16_t servo3_raw; ///< Servo output 3 value, in microseconds - uint16_t servo4_raw; ///< Servo output 4 value, in microseconds - uint16_t servo5_raw; ///< Servo output 5 value, in microseconds - uint16_t servo6_raw; ///< Servo output 6 value, in microseconds - uint16_t servo7_raw; ///< Servo output 7 value, in microseconds - uint16_t servo8_raw; ///< Servo output 8 value, in microseconds + uint16_t servo1_raw; ///< Servo output 1 value, in microseconds + uint16_t servo2_raw; ///< Servo output 2 value, in microseconds + uint16_t servo3_raw; ///< Servo output 3 value, in microseconds + uint16_t servo4_raw; ///< Servo output 4 value, in microseconds + uint16_t servo5_raw; ///< Servo output 5 value, in microseconds + uint16_t servo6_raw; ///< Servo output 6 value, in microseconds + uint16_t servo7_raw; ///< Servo output 7 value, in microseconds + uint16_t servo8_raw; ///< Servo output 8 value, in microseconds } mavlink_servo_output_raw_t; @@ -38,14 +40,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds - p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds - p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds - p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds - p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds - p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds - p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds - p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } @@ -71,14 +73,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds - p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds - p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds - p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds - p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds - p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds - p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds - p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); } @@ -96,6 +98,8 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a servo_output_raw message * @param chan MAVLink channel to send the message @@ -109,24 +113,20 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui * @param servo7_raw Servo output 7 value, in microseconds * @param servo8_raw Servo output 8 value, in microseconds */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { mavlink_header_t hdr; mavlink_servo_output_raw_t payload; - uint16_t checksum; - mavlink_servo_output_raw_t *p = &payload; - - p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds - p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds - p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds - p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds - p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds - p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds - p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds - p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN ) + payload.servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds + payload.servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds + payload.servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds + payload.servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds + payload.servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds + payload.servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds + payload.servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds + payload.servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; @@ -137,14 +137,12 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xEA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 9b50027f1..a4fea868c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_SET_ALTITUDE 65 #define MAVLINK_MSG_ID_SET_ALTITUDE_LEN 5 #define MAVLINK_MSG_65_LEN 5 +#define MAVLINK_MSG_ID_SET_ALTITUDE_KEY 0x12 +#define MAVLINK_MSG_65_KEY 0x12 typedef struct __mavlink_set_altitude_t { - uint32_t mode; ///< The new altitude in meters - uint8_t target; ///< The system setting the altitude + uint32_t mode; ///< The new altitude in meters + uint8_t target; ///< The system setting the altitude } mavlink_set_altitude_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_set_altitude_pack(uint8_t system_id, uint8_t mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_set_altitude_pack_chan(uint8_t system_id, uin mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters + p->target = target; // uint8_t:The system setting the altitude + p->mode = mode; // uint32_t:The new altitude in meters return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ return mavlink_msg_set_altitude_pack(system_id, component_id, msg, set_altitude->target, set_altitude->mode); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_altitude message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ * @param target The system setting the altitude * @param mode The new altitude in meters */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) { mavlink_header_t hdr; mavlink_set_altitude_t payload; - uint16_t checksum; - mavlink_set_altitude_t *p = &payload; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_ALTITUDE_LEN ) + payload.target = target; // uint8_t:The system setting the altitude + payload.mode = mode; // uint32_t:The new altitude in meters hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x12, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index ad796f1e0..85e2d4b9f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_SET_MODE 11 #define MAVLINK_MSG_ID_SET_MODE_LEN 2 #define MAVLINK_MSG_11_LEN 2 +#define MAVLINK_MSG_ID_SET_MODE_KEY 0xF9 +#define MAVLINK_MSG_11_KEY 0xF9 typedef struct __mavlink_set_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t mode; ///< The new mode + uint8_t target; ///< The system setting the mode + uint8_t mode; ///< The new mode } mavlink_set_mode_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_set_mode_pack(uint8_t system_id, uint8_t comp mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_MODE_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_set_mode_pack_chan(uint8_t system_id, uint8_t mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_MODE; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode + p->target = target; // uint8_t:The system setting the mode + p->mode = mode; // uint8_t:The new mode return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_MODE_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co return mavlink_msg_set_mode_pack(system_id, component_id, msg, set_mode->target, set_mode->mode); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_mode message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co * @param target The system setting the mode * @param mode The new mode */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { mavlink_header_t hdr; mavlink_set_mode_t payload; - uint16_t checksum; - mavlink_set_mode_t *p = &payload; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_MODE_LEN ) + payload.target = target; // uint8_t:The system setting the mode + payload.mode = mode; // uint8_t:The new mode hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SET_MODE_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t tar mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xF9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h index 114744009..f42432e3a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_SET_NAV_MODE 12 #define MAVLINK_MSG_ID_SET_NAV_MODE_LEN 2 #define MAVLINK_MSG_12_LEN 2 +#define MAVLINK_MSG_ID_SET_NAV_MODE_KEY 0x85 +#define MAVLINK_MSG_12_KEY 0x85 typedef struct __mavlink_set_nav_mode_t { - uint8_t target; ///< The system setting the mode - uint8_t nav_mode; ///< The new navigation mode + uint8_t target; ///< The system setting the mode + uint8_t nav_mode; ///< The new navigation mode } mavlink_set_nav_mode_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack(uint8_t system_id, uint8_t mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_set_nav_mode_pack_chan(uint8_t system_id, uin mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode + p->target = target; // uint8_t:The system setting the mode + p->nav_mode = nav_mode; // uint8_t:The new navigation mode return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ return mavlink_msg_set_nav_mode_pack(system_id, component_id, msg, set_nav_mode->target, set_nav_mode->nav_mode); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_nav_mode message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ * @param target The system setting the mode * @param nav_mode The new navigation mode */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) { mavlink_header_t hdr; mavlink_set_nav_mode_t payload; - uint16_t checksum; - mavlink_set_nav_mode_t *p = &payload; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_NAV_MODE_LEN ) + payload.target = target; // uint8_t:The system setting the mode + payload.nav_mode = nav_mode; // uint8_t:The new navigation mode hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x85, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 new file mode 100644 index 000000000..e2d6e9afc --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h @@ -0,0 +1,212 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 56 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN 18 +#define MAVLINK_MSG_56_LEN 18 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_KEY 0x74 +#define MAVLINK_MSG_56_KEY 0x74 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t +{ + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + float thrust; ///< Collective thrust, normalized to 0 .. 1 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_set_roll_pitch_yaw_speed_thrust_t; + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust); +} + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief Send a set_roll_pitch_yaw_speed_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +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_header_t hdr; + mavlink_set_roll_pitch_yaw_speed_thrust_t payload; + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + payload.pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + payload.yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x74, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_thrust_t *p = (mavlink_set_roll_pitch_yaw_speed_thrust_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust) +{ + memcpy( set_roll_pitch_yaw_speed_thrust, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_thrust_t)); +} 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 new file mode 100644 index 000000000..5d8e5f8ea --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h @@ -0,0 +1,212 @@ +// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 55 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN 18 +#define MAVLINK_MSG_55_LEN 18 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_KEY 0xA1 +#define MAVLINK_MSG_55_KEY 0xA1 + +typedef struct __mavlink_set_roll_pitch_yaw_thrust_t +{ + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + float thrust; ///< Collective thrust, normalized to 0 .. 1 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_set_roll_pitch_yaw_thrust_t; + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw_thrust message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + p->thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw_thrust struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust); +} + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +/** + * @brief Send a set_roll_pitch_yaw_thrust message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @param thrust Collective thrust, normalized to 0 .. 1 + */ +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_header_t hdr; + mavlink_set_roll_pitch_yaw_thrust_t payload; + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.roll = roll; // float:Desired roll angle in radians + payload.pitch = pitch; // float:Desired pitch angle in radians + payload.yaw = yaw; // float:Desired yaw angle in radians + payload.thrust = thrust; // float:Collective thrust, normalized to 0 .. 1 + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA1, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_thrust message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_thrust message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll from set_roll_pitch_yaw_thrust message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw_thrust message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw_thrust message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Get field thrust from set_roll_pitch_yaw_thrust message + * + * @return Collective thrust, normalized to 0 .. 1 + */ +static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_thrust_t *p = (mavlink_set_roll_pitch_yaw_thrust_t *)&msg->payload[0]; + return (float)(p->thrust); +} + +/** + * @brief Decode a set_roll_pitch_yaw_thrust message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust) +{ + memcpy( set_roll_pitch_yaw_thrust, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_thrust_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index f101513eb..f92a13265 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_STATE_CORRECTION 64 #define MAVLINK_MSG_ID_STATE_CORRECTION_LEN 36 #define MAVLINK_MSG_64_LEN 36 +#define MAVLINK_MSG_ID_STATE_CORRECTION_KEY 0xB9 +#define MAVLINK_MSG_64_KEY 0xB9 typedef struct __mavlink_state_correction_t { - float xErr; ///< x position error - float yErr; ///< y position error - float zErr; ///< z position error - float rollErr; ///< roll error (radians) - float pitchErr; ///< pitch error (radians) - float yawErr; ///< yaw error (radians) - float vxErr; ///< x velocity - float vyErr; ///< y velocity - float vzErr; ///< z velocity + float xErr; ///< x position error + float yErr; ///< y position error + float zErr; ///< z position error + float rollErr; ///< roll error (radians) + float pitchErr; ///< pitch error (radians) + float yawErr; ///< yaw error (radians) + float vxErr; ///< x velocity + float vyErr; ///< y velocity + float vzErr; ///< z velocity } mavlink_state_correction_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_state_correction_pack(uint8_t system_id, uint mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - p->xErr = xErr; // float:x position error - p->yErr = yErr; // float:y position error - p->zErr = zErr; // float:z position error - p->rollErr = rollErr; // float:roll error (radians) - p->pitchErr = pitchErr; // float:pitch error (radians) - p->yawErr = yawErr; // float:yaw error (radians) - p->vxErr = vxErr; // float:x velocity - p->vyErr = vyErr; // float:y velocity - p->vzErr = vzErr; // float:z velocity + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_state_correction_pack_chan(uint8_t system_id, mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - p->xErr = xErr; // float:x position error - p->yErr = yErr; // float:y position error - p->zErr = zErr; // float:z position error - p->rollErr = rollErr; // float:roll error (radians) - p->pitchErr = pitchErr; // float:pitch error (radians) - p->yawErr = yawErr; // float:yaw error (radians) - p->vxErr = vxErr; // float:x velocity - p->vyErr = vyErr; // float:y velocity - p->vzErr = vzErr; // float:z velocity + p->xErr = xErr; // float:x position error + p->yErr = yErr; // float:y position error + p->zErr = zErr; // float:z position error + p->rollErr = rollErr; // float:roll error (radians) + p->pitchErr = pitchErr; // float:pitch error (radians) + p->yawErr = yawErr; // float:yaw error (radians) + p->vxErr = vxErr; // float:x velocity + p->vyErr = vyErr; // float:y velocity + p->vzErr = vzErr; // float:z velocity return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui return mavlink_msg_state_correction_pack(system_id, component_id, msg, state_correction->xErr, state_correction->yErr, state_correction->zErr, state_correction->rollErr, state_correction->pitchErr, state_correction->yawErr, state_correction->vxErr, state_correction->vyErr, state_correction->vzErr); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a state_correction message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui * @param vyErr y velocity * @param vzErr z velocity */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { mavlink_header_t hdr; mavlink_state_correction_t payload; - uint16_t checksum; - mavlink_state_correction_t *p = &payload; - p->xErr = xErr; // float:x position error - p->yErr = yErr; // float:y position error - p->zErr = zErr; // float:z position error - p->rollErr = rollErr; // float:roll error (radians) - p->pitchErr = pitchErr; // float:pitch error (radians) - p->yawErr = yawErr; // float:yaw error (radians) - p->vxErr = vxErr; // float:x velocity - p->vyErr = vyErr; // float:y velocity - p->vzErr = vzErr; // float:z velocity + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_STATE_CORRECTION_LEN ) + payload.xErr = xErr; // float:x position error + payload.yErr = yErr; // float:y position error + payload.zErr = zErr; // float:z position error + payload.rollErr = rollErr; // float:roll error (radians) + payload.pitchErr = pitchErr; // float:pitch error (radians) + payload.yawErr = yawErr; // float:yaw error (radians) + payload.vxErr = vxErr; // float:x velocity + payload.vyErr = vyErr; // float:y velocity + payload.vzErr = vzErr; // float:z velocity hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, flo mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xB9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index e53bc5390..beb411483 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_STATUSTEXT 254 #define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 #define MAVLINK_MSG_254_LEN 51 +#define MAVLINK_MSG_ID_STATUSTEXT_KEY 0x90 +#define MAVLINK_MSG_254_KEY 0x90 typedef struct __mavlink_statustext_t { - uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault - char text[50]; ///< Status text message, without null termination character + uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault + char text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; - p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -67,6 +69,8 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a statustext message * @param chan MAVLink channel to send the message @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param severity Severity of status, 0 = info message, 255 = critical fault * @param text Status text message, without null termination character */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) { mavlink_header_t hdr; mavlink_statustext_t payload; - uint16_t checksum; - mavlink_statustext_t *p = &payload; - p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_STATUSTEXT_LEN ) + payload.severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault + memcpy(payload.text, text, sizeof(payload.text)); // char[50]:Status text message, without null termination character hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; @@ -96,14 +96,12 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x90, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index 1ca8cec2e..503cf19a0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_SYS_STATUS 34 #define MAVLINK_MSG_ID_SYS_STATUS_LEN 11 #define MAVLINK_MSG_34_LEN 11 +#define MAVLINK_MSG_ID_SYS_STATUS_KEY 0x6F +#define MAVLINK_MSG_34_KEY 0x6F typedef struct __mavlink_sys_status_t { - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) - uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) - uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) - uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - uint8_t status; ///< System status flag, see MAV_STATUS ENUM + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) + uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) + uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) + uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + uint8_t status; ///< System status flag, see MAV_STATUS ENUM } mavlink_sys_status_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t co mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8 mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYS_STATUS; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->mode, sys_status->nav_mode, sys_status->status, sys_status->load, sys_status->vbat, sys_status->battery_remaining, sys_status->packet_drop); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a sys_status message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_header_t hdr; mavlink_sys_status_t payload; - uint16_t checksum; - mavlink_sys_status_t *p = &payload; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYS_STATUS_LEN ) + payload.mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + payload.nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM + payload.status = status; // uint8_t:System status flag, see MAV_STATUS ENUM + payload.load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + payload.vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) + payload.battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) + payload.packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t m mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x6F, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index c2328a68f..790be507a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_SYSTEM_TIME 2 #define MAVLINK_MSG_ID_SYSTEM_TIME_LEN 8 #define MAVLINK_MSG_2_LEN 8 +#define MAVLINK_MSG_ID_SYSTEM_TIME_KEY 0xE8 +#define MAVLINK_MSG_2_KEY 0xE8 typedef struct __mavlink_system_time_t { - uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. + uint64_t time_usec; ///< Timestamp of the master clock in microseconds since UNIX epoch. } mavlink_system_time_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_system_time_pack(uint8_t system_id, uint8_t c mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_system_time_pack_chan(uint8_t system_id, uint mavlink_system_time_t *p = (mavlink_system_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN); } @@ -61,23 +63,21 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t return mavlink_msg_system_time_pack(system_id, component_id, msg, system_time->time_usec); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a system_time message * @param chan MAVLink channel to send the message * * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { mavlink_header_t hdr; mavlink_system_time_t payload; - uint16_t checksum; - mavlink_system_time_t *p = &payload; - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYSTEM_TIME_LEN ) + payload.time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; @@ -88,14 +88,12 @@ static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xE8, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 414b4984f..5ee9fcb9d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC 4 #define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN 8 #define MAVLINK_MSG_4_LEN 8 +#define MAVLINK_MSG_ID_SYSTEM_TIME_UTC_KEY 0x4C +#define MAVLINK_MSG_4_KEY 0x4C typedef struct __mavlink_system_time_utc_t { - uint32_t utc_date; ///< GPS UTC date ddmmyy - uint32_t utc_time; ///< GPS UTC time hhmmss + uint32_t utc_date; ///< GPS UTC date ddmmyy + uint32_t utc_time; ///< GPS UTC time hhmmss } mavlink_system_time_utc_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_system_time_utc_pack(uint8_t system_id, uint8 mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_system_time_utc_pack_chan(uint8_t system_id, mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin return mavlink_msg_system_time_utc_pack(system_id, component_id, msg, system_time_utc->utc_date, system_time_utc->utc_time); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a system_time_utc message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin * @param utc_date GPS UTC date ddmmyy * @param utc_time GPS UTC time hhmmss */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { mavlink_header_t hdr; mavlink_system_time_utc_t payload; - uint16_t checksum; - mavlink_system_time_utc_t *p = &payload; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN ) + payload.utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy + payload.utc_time = utc_time; // uint32_t:GPS UTC time hhmmss hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x4C, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index 0b757cef6..abb6fdcf1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_VFR_HUD 74 #define MAVLINK_MSG_ID_VFR_HUD_LEN 20 #define MAVLINK_MSG_74_LEN 20 +#define MAVLINK_MSG_ID_VFR_HUD_KEY 0xFB +#define MAVLINK_MSG_74_KEY 0xFB typedef struct __mavlink_vfr_hud_t { - float airspeed; ///< Current airspeed in m/s - float groundspeed; ///< Current ground speed in m/s - float alt; ///< Current altitude (MSL), in meters - float climb; ///< Current climb rate in meters/second - int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 + float airspeed; ///< Current airspeed in m/s + float groundspeed; ///< Current ground speed in m/s + float alt; ///< Current altitude (MSL), in meters + float climb; ///< Current climb rate in meters/second + int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 } mavlink_vfr_hud_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_vfr_hud_pack(uint8_t system_id, uint8_t compo mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - p->airspeed = airspeed; // float:Current airspeed in m/s - p->groundspeed = groundspeed; // float:Current ground speed in m/s - p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) - p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 - p->alt = alt; // float:Current altitude (MSL), in meters - p->climb = climb; // float:Current climb rate in meters/second + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VFR_HUD_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_vfr_hud_pack_chan(uint8_t system_id, uint8_t mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VFR_HUD; - p->airspeed = airspeed; // float:Current airspeed in m/s - p->groundspeed = groundspeed; // float:Current ground speed in m/s - p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) - p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 - p->alt = alt; // float:Current altitude (MSL), in meters - p->climb = climb; // float:Current climb rate in meters/second + p->airspeed = airspeed; // float:Current airspeed in m/s + p->groundspeed = groundspeed; // float:Current ground speed in m/s + p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + p->alt = alt; // float:Current altitude (MSL), in meters + p->climb = climb; // float:Current climb rate in meters/second return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com return mavlink_msg_vfr_hud_pack(system_id, component_id, msg, vfr_hud->airspeed, vfr_hud->groundspeed, vfr_hud->heading, vfr_hud->throttle, vfr_hud->alt, vfr_hud->climb); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vfr_hud message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com * @param alt Current altitude (MSL), in meters * @param climb Current climb rate in meters/second */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { mavlink_header_t hdr; mavlink_vfr_hud_t payload; - uint16_t checksum; - mavlink_vfr_hud_t *p = &payload; - p->airspeed = airspeed; // float:Current airspeed in m/s - p->groundspeed = groundspeed; // float:Current ground speed in m/s - p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) - p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 - p->alt = alt; // float:Current altitude (MSL), in meters - p->climb = climb; // float:Current climb rate in meters/second + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VFR_HUD_LEN ) + payload.airspeed = airspeed; // float:Current airspeed in m/s + payload.groundspeed = groundspeed; // float:Current ground speed in m/s + payload.heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) + payload.throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 + payload.alt = alt; // float:Current altitude (MSL), in meters + payload.climb = climb; // float:Current climb rate in meters/second hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_VFR_HUD_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspe mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xFB, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index a29a26b0e..71ac9f7f3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -3,23 +3,25 @@ #define MAVLINK_MSG_ID_WAYPOINT 39 #define MAVLINK_MSG_ID_WAYPOINT_LEN 36 #define MAVLINK_MSG_39_LEN 36 +#define MAVLINK_MSG_ID_WAYPOINT_KEY 0xC5 +#define MAVLINK_MSG_39_KEY 0xC5 typedef struct __mavlink_waypoint_t { - float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - float x; ///< PARAM5 / local: x position, global: latitude - float y; ///< PARAM6 / y position: global: longitude - float z; ///< PARAM7 / z position: global: altitude - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp + float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + float param4; ///< PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + float x; ///< PARAM5 / local: x position, global: latitude + float y; ///< PARAM6 / y position: global: longitude + float z; ///< PARAM7 / z position: global: altitude + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp } mavlink_waypoint_t; @@ -50,20 +52,20 @@ static inline uint16_t mavlink_msg_waypoint_pack(uint8_t system_id, uint8_t comp mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - p->current = current; // uint8_t:false:0, true:1 - p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp - p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - p->x = x; // float:PARAM5 / local: x position, global: latitude - p->y = y; // float:PARAM6 / y position: global: longitude - p->z = z; // float:PARAM7 / z position: global: altitude + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_LEN); } @@ -95,20 +97,20 @@ static inline uint16_t mavlink_msg_waypoint_pack_chan(uint8_t system_id, uint8_t mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - p->current = current; // uint8_t:false:0, true:1 - p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp - p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - p->x = x; // float:PARAM5 / local: x position, global: latitude - p->y = y; // float:PARAM6 / y position: global: longitude - p->z = z; // float:PARAM7 / z position: global: altitude + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence + p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + p->current = current; // uint8_t:false:0, true:1 + p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp + p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + p->x = x; // float:PARAM5 / local: x position, global: latitude + p->y = y; // float:PARAM6 / y position: global: longitude + p->z = z; // float:PARAM7 / z position: global: altitude return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_LEN); } @@ -126,6 +128,8 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co return mavlink_msg_waypoint_pack(system_id, component_id, msg, waypoint->target_system, waypoint->target_component, waypoint->seq, waypoint->frame, waypoint->command, waypoint->current, waypoint->autocontinue, waypoint->param1, waypoint->param2, waypoint->param3, waypoint->param4, waypoint->x, waypoint->y, waypoint->z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint message * @param chan MAVLink channel to send the message @@ -145,30 +149,26 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co * @param y PARAM6 / y position: global: longitude * @param z PARAM7 / z position: global: altitude */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { mavlink_header_t hdr; mavlink_waypoint_t payload; - uint16_t checksum; - mavlink_waypoint_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - p->current = current; // uint8_t:false:0, true:1 - p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp - p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - p->x = x; // float:PARAM5 / local: x position, global: latitude - p->y = y; // float:PARAM6 / y position: global: longitude - p->z = z; // float:PARAM7 / z position: global: altitude + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.seq = seq; // uint16_t:Sequence + payload.frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + payload.command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + payload.current = current; // uint8_t:false:0, true:1 + payload.autocontinue = autocontinue; // uint8_t:autocontinue to next wp + payload.param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + payload.param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + payload.param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + payload.param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + payload.x = x; // float:PARAM5 / local: x position, global: latitude + payload.y = y; // float:PARAM6 / y position: global: longitude + payload.z = z; // float:PARAM7 / z position: global: altitude hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_LEN; @@ -179,14 +179,12 @@ static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t tar mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xC5, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 8a5d52ed3..2ea2aae69 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_WAYPOINT_ACK 47 #define MAVLINK_MSG_ID_WAYPOINT_ACK_LEN 3 #define MAVLINK_MSG_47_LEN 3 +#define MAVLINK_MSG_ID_WAYPOINT_ACK_KEY 0x9E +#define MAVLINK_MSG_47_KEY 0x9E typedef struct __mavlink_waypoint_ack_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t type; ///< 0: OK, 1: Error + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t type; ///< 0: OK, 1: Error } mavlink_waypoint_ack_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack(uint8_t system_id, uint8_t mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->type = type; // uint8_t:0: OK, 1: Error + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_ack_pack_chan(uint8_t system_id, uin mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->type = type; // uint8_t:0: OK, 1: Error + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->type = type; // uint8_t:0: OK, 1: Error return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ return mavlink_msg_waypoint_ack_pack(system_id, component_id, msg, waypoint_ack->target_system, waypoint_ack->target_component, waypoint_ack->type); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_ack message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ * @param target_component Component ID * @param type 0: OK, 1: Error */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { mavlink_header_t hdr; mavlink_waypoint_ack_t payload; - uint16_t checksum; - mavlink_waypoint_ack_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->type = type; // uint8_t:0: OK, 1: Error + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_ACK_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.type = type; // uint8_t:0: OK, 1: Error hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x9E, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 b96a80696..8e7a1a9c2 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL 45 #define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN 2 #define MAVLINK_MSG_45_LEN 2 +#define MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_KEY 0x22 +#define MAVLINK_MSG_45_KEY 0x22 typedef struct __mavlink_waypoint_clear_all_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_clear_all_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack(uint8_t system_id, ui mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_pack_chan(uint8_t system_i mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, return mavlink_msg_waypoint_clear_all_pack(system_id, component_id, msg, waypoint_clear_all->target_system, waypoint_clear_all->target_component); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_clear_all message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, * @param target_system System ID * @param target_component Component ID */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { mavlink_header_t hdr; mavlink_waypoint_clear_all_t payload; - uint16_t checksum; - mavlink_waypoint_clear_all_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, u mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x22, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index 8856c3c44..19cea8169 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_WAYPOINT_COUNT 44 #define MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN 4 #define MAVLINK_MSG_44_LEN 4 +#define MAVLINK_MSG_ID_WAYPOINT_COUNT_KEY 0xE9 +#define MAVLINK_MSG_44_KEY 0xE9 typedef struct __mavlink_waypoint_count_t { - uint16_t count; ///< Number of Waypoints in the Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint16_t count; ///< Number of Waypoints in the Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_count_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_count_pack(uint8_t system_id, uint8_ mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->count = count; // uint16_t:Number of Waypoints in the Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_count_pack_chan(uint8_t system_id, u mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->count = count; // uint16_t:Number of Waypoints in the Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->count = count; // uint16_t:Number of Waypoints in the Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint return mavlink_msg_waypoint_count_pack(system_id, component_id, msg, waypoint_count->target_system, waypoint_count->target_component, waypoint_count->count); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_count message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint * @param target_component Component ID * @param count Number of Waypoints in the Sequence */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { mavlink_header_t hdr; mavlink_waypoint_count_t payload; - uint16_t checksum; - mavlink_waypoint_count_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->count = count; // uint16_t:Number of Waypoints in the Sequence + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.count = count; // uint16_t:Number of Waypoints in the Sequence hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8 mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xE9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 9c75639c7..b3ed6ce31 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_WAYPOINT_CURRENT 42 #define MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN 2 #define MAVLINK_MSG_42_LEN 2 +#define MAVLINK_MSG_ID_WAYPOINT_CURRENT_KEY 0xA6 +#define MAVLINK_MSG_42_KEY 0xA6 typedef struct __mavlink_waypoint_current_t { - uint16_t seq; ///< Sequence + uint16_t seq; ///< Sequence } mavlink_waypoint_current_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_waypoint_current_pack(uint8_t system_id, uint mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - p->seq = seq; // uint16_t:Sequence + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_waypoint_current_pack_chan(uint8_t system_id, mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - p->seq = seq; // uint16_t:Sequence + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN); } @@ -61,23 +63,21 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui return mavlink_msg_waypoint_current_pack(system_id, component_id, msg, waypoint_current->seq); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_current message * @param chan MAVLink channel to send the message * * @param seq Sequence */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { mavlink_header_t hdr; mavlink_waypoint_current_t payload; - uint16_t checksum; - mavlink_waypoint_current_t *p = &payload; - p->seq = seq; // uint16_t:Sequence + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN ) + payload.seq = seq; // uint16_t:Sequence hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; @@ -88,14 +88,12 @@ static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA6, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 5e1f144e8..801416191 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_WAYPOINT_REACHED 46 #define MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN 2 #define MAVLINK_MSG_46_LEN 2 +#define MAVLINK_MSG_ID_WAYPOINT_REACHED_KEY 0xA6 +#define MAVLINK_MSG_46_KEY 0xA6 typedef struct __mavlink_waypoint_reached_t { - uint16_t seq; ///< Sequence + uint16_t seq; ///< Sequence } mavlink_waypoint_reached_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack(uint8_t system_id, uint mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - p->seq = seq; // uint16_t:Sequence + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_waypoint_reached_pack_chan(uint8_t system_id, mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - p->seq = seq; // uint16_t:Sequence + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN); } @@ -61,23 +63,21 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui return mavlink_msg_waypoint_reached_pack(system_id, component_id, msg, waypoint_reached->seq); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_reached message * @param chan MAVLink channel to send the message * * @param seq Sequence */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { mavlink_header_t hdr; mavlink_waypoint_reached_t payload; - uint16_t checksum; - mavlink_waypoint_reached_t *p = &payload; - p->seq = seq; // uint16_t:Sequence + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN ) + payload.seq = seq; // uint16_t:Sequence hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; @@ -88,14 +88,12 @@ static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA6, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index 9f3ccdbe3..da321fc17 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_WAYPOINT_REQUEST 40 #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN 4 #define MAVLINK_MSG_40_LEN 4 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_KEY 0xC0 +#define MAVLINK_MSG_40_KEY 0xC0 typedef struct __mavlink_waypoint_request_t { - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_request_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_request_pack(uint8_t system_id, uint mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_request_pack_chan(uint8_t system_id, mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui return mavlink_msg_waypoint_request_pack(system_id, component_id, msg, waypoint_request->target_system, waypoint_request->target_component, waypoint_request->seq); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_request message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui * @param target_component Component ID * @param seq Sequence */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { mavlink_header_t hdr; mavlink_waypoint_request_t payload; - uint16_t checksum; - mavlink_waypoint_request_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.seq = seq; // uint16_t:Sequence hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uin mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xC0, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 34d8276d0..44efd58f4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST 43 #define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN 2 #define MAVLINK_MSG_43_LEN 2 +#define MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_KEY 0x22 +#define MAVLINK_MSG_43_KEY 0x22 typedef struct __mavlink_waypoint_request_list_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_request_list_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack(uint8_t system_id, mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_waypoint_request_list_pack_chan(uint8_t syste mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i return mavlink_msg_waypoint_request_list_pack(system_id, component_id, msg, waypoint_request_list->target_system, waypoint_request_list->target_component); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_request_list message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i * @param target_system System ID * @param target_component Component ID */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { mavlink_header_t hdr; mavlink_waypoint_request_list_t payload; - uint16_t checksum; - mavlink_waypoint_request_list_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x22, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #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 829f29982..994a0dc6d 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT 41 #define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN 4 #define MAVLINK_MSG_41_LEN 4 +#define MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_KEY 0xC0 +#define MAVLINK_MSG_41_KEY 0xC0 typedef struct __mavlink_waypoint_set_current_t { - uint16_t seq; ///< Sequence - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_waypoint_set_current_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack(uint8_t system_id, mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_waypoint_set_current_pack_chan(uint8_t system mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->seq = seq; // uint16_t:Sequence return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id return mavlink_msg_waypoint_set_current_pack(system_id, component_id, msg, waypoint_set_current->target_system, waypoint_set_current->target_component, waypoint_set_current->seq); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a waypoint_set_current message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id * @param target_component Component ID * @param seq Sequence */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { mavlink_header_t hdr; mavlink_waypoint_set_current_t payload; - uint16_t checksum; - mavlink_waypoint_set_current_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.seq = seq; // uint16_t:Sequence hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); - crc_init(&checksum); - checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); - hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); - mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xC0, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/mavlink_checksum.h b/thirdParty/mavlink/include/mavlink_checksum.h old mode 100755 new mode 100644 index 0f4225174..fdcee99d1 --- a/thirdParty/mavlink/include/mavlink_checksum.h +++ b/thirdParty/mavlink/include/mavlink_checksum.h @@ -1,156 +1,183 @@ -/** @file - * @brief MAVLink comm protocol checksum routines. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _CHECKSUM_H_ -#define _CHECKSUM_H_ - -#include "inttypes.h" - - -/** - * - * CALCULATE THE CHECKSUM - * - */ - -#define X25_INIT_CRC 0xffff -#define X25_VALIDATE_CRC 0xf0b8 - -/** - * @brief Accumulate the X.25 CRC by adding one char at a time. - * - * The checksum function adds the hash of one char at a time to the - * 16 bit checksum (uint16_t). - * - * @param data new char to hash - * @param crcAccum the already accumulated checksum - **/ -static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) -{ - /*Accumulate one byte of data into the CRC*/ - uint8_t tmp; - - tmp=data ^ (uint8_t)(*crcAccum &0xff); - tmp^= (tmp<<4); - *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); -// *crcAccum += data; // super simple to test -} - -/** - * @brief Initiliaze the buffer for the X.25 CRC - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init(uint16_t* crcAccum) -{ - *crcAccum = X25_INIT_CRC; -} - -/** - * @brief Initiliaze the buffer for the X.25 CRC to a specified value - * - * @param crcAccum the 16 bit X.25 CRC - */ -static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) -{ - *crcAccum = crcValue; -} - - - -/** - * @brief Calculates the X.25 checksum on a byte buffer - * - * @param pBuffer buffer containing the byte array to hash - * @param length length of the byte array - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) -{ - - // For a "message" of length bytes contained in the unsigned char array - // pointed to by pBuffer, calculate the CRC - // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed - - uint16_t crcTmp; - //uint16_t tmp; - uint8_t* pTmp; - int i; - - pTmp=pBuffer; - - - /* init crcTmp */ - crc_init(&crcTmp); - - for (i = 0; i < length; i++){ - crc_accumulate(*pTmp++, &crcTmp); - } - - /* This is currently not needed, as only the checksum over payload should be computed - tmp = crcTmp; - crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); - crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); - *checkConst = tmp; - */ - return(crcTmp); -} - - -/** - * @brief Calculates the X.25 checksum on a msg buffer - * - * @param pMSG buffer containing the msg to hash - * @param length number of bytes to hash - * @return the checksum over the buffer bytes - **/ -static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) -{ - - // For a "message" of length bytes contained in the unsigned char array - // pointed to by pBuffer, calculate the CRC - // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed - - uint16_t crcTmp; - //uint16_t tmp; - uint8_t* pTmp; - int i; - - pTmp=&pMSG->len; - - /* init crcTmp */ - crc_init(&crcTmp); - - for (i = 0; i < 5; i++){ - crc_accumulate(*pTmp++, &crcTmp); - } - - pTmp=&pMSG->payload[0]; - for (; i < length; i++){ - crc_accumulate(*pTmp++, &crcTmp); - } - - /* This is currently not needed, as only the checksum over payload should be computed - tmp = crcTmp; - crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); - crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); - *checkConst = tmp; - */ - return(crcTmp); -} - - - - -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus -} -#endif +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_mem(uint8_t *pBuffer, uint16_t *crcTmp, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + //uint16_t tmp; + //uint8_t* pTmp; + int i; + +// pTmp=pBuffer; + + for (i = 0; i < length; i++){ + crc_accumulate(*pBuffer++, crcTmp); + } + + return(*crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_data.h b/thirdParty/mavlink/include/mavlink_data.h old mode 100755 new mode 100644 index ce51e2b93..17c88cc2a --- a/thirdParty/mavlink/include/mavlink_data.h +++ b/thirdParty/mavlink/include/mavlink_data.h @@ -1,21 +1,21 @@ -/** @file - * @brief Main MAVLink comm protocol data. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifndef _ML_DATA_H_ -#define _ML_DATA_H_ - -#include "mavlink_types.h" - -#ifdef MAVLINK_CHECK_LENGTH -const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -#endif - -const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; - -mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +mavlink_system_t mavlink_system; #endif \ No newline at end of file diff --git a/thirdParty/mavlink/include/mavlink_options.h b/thirdParty/mavlink/include/mavlink_options.h old mode 100755 new mode 100644 index bc337f617..550a85ae5 --- a/thirdParty/mavlink/include/mavlink_options.h +++ b/thirdParty/mavlink/include/mavlink_options.h @@ -1,102 +1,135 @@ -/** @file - * @brief MAVLink comm protocol option constants. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef _ML_OPTIONS_H_ -#define _ML_OPTIONS_H_ - - -/** - * - * Receive message length check option. On receive verify the length field - * as soon as the message ID field is received. Requires a 256 byte const - * table. Comment out the define to leave out the table and code to check it. - * - */ -//#define MAVLINK_CHECK_LENGTH - -/** - * - * Receive message buffer option. This option should be used only when the - * side effects are understood but allows the underlying program access to - * the internal recieve buffer - eliminating the usual double buffering. It - * also REQUIRES changes in the return type of mavlink_parse_char so only - * enable if you make the changes required. Default DISABLED. - * - */ -//#define MAVLINK_STATIC_BUFFER - -/** - * - * Receive message buffers option. This option defines how many msg buffers - * mavlink will define, and thereby how many links it can support. A default - * will be supplied if the symbol is not pre-defined, dependant on the make - * envionment. The default is 16 for a recognised OS envionment and 1 - * otherwise. - * - */ -#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) -#undef MAVLINK_COMM_NB - #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) - #define MAVLINK_COMM_NB 16 - #else - #define MAVLINK_COMM_NB 1 - #endif -#endif - - -/** - * - * Data relization option. This option controls inclusion of the file - * mavlink_data.h in the current compile unit - thus defining mavlink's - * variables. Default is ON (not defined) because typically mavlink.h is only - * included once in a system but if it was used in two files there would - * be duplicate variables at link time. Normal practice would be to define - * this symbol outside of this file as defining it here will cause missing - * symbols at link time. In other words in the first file to include mavlink.h - * do not define this sybol, then define this symbol in all other files before - * including mavlink.h - * - */ -//#define MAVLINK_NO_DATA -#ifdef MAVLINK_NO_DATA - #undef MAVLINK_DATA -#else - #define MAVLINK_DATA -#endif - -/** - * - * Custom data const data relization and access options. - * This define is placed in the form - * const uint8_t MAVLINK_CONST name[] = { ... }; - * for the keys table and (if applicable) lengths table to tell the compiler - * were to put the data. The access option is placed in the form - * variable = MAVLINK_CONST_READ( name[i] ); - * in order to allow custom read function's or accessors. - * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as - * MAVLINK_CONST_READ( a ) a - * These symbols are only defined if not already defined allowing this file - * to remain unchanged while the actual definitions are maintained in external - * files. - * - */ -#ifndef MAVLINK_CONST -#define MAVLINK_CONST -#endif -#ifndef MAVLINK_CONST_READ -#define MAVLINK_CONST_READ( a ) a -#endif - - -#endif /* _ML_OPTIONS_H_ */ - -#ifdef __cplusplus -} -#endif +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +//#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STATIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +/** + * + * Convience functions. These are all in one send functions that are very + * easy to use. Just define the symbol MAVLINK_USE_CONVENIENCE_FUNCTIONS. + * These functions also support a buffer check, to ensure there is enough + * space in your comm buffer that the function would not block - it could + * also be used as the basis of a MUTEX. This is implemented in the send + * function as a macro with two arguments, first the comm chan number and + * the message length in the form + * MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_LEN ) + * followed by the function code and then + * MAVLINK_BUFFER_CHECK_START + * Note that there are no terminators on these statements to allow for + * code nesting or other constructs. Default value for both is empty. + * A sugested implementation is shown below and the symbols will be defined + * only if they are not allready. + * + * if ( serial_space( chan ) > len ) { // serial_space returns available space + * ..... code that creates message + * } + * + * #define MAVLINK_BUFFER_CHECK_START( c, l ) if ( serial_space( c ) > l ) { + * #define MAVLINK_BUFFER_CHECK_END } + * + */ +//#define MAVLINK_USE_CONVENIENCE_FUNCTIONS +#ifndef MAVLINK_BUFFER_CHECK_START +#define MAVLINK_BUFFER_CHECK_START( c, l ) ; +#endif +#ifndef MAVLINK_BUFFER_CHECK_END +#define MAVLINK_BUFFER_CHECK_END ; +#endif + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h old mode 100755 new mode 100644 index fe63af913..596b5ffcf --- a/thirdParty/mavlink/include/mavlink_protocol.h +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -1,424 +1,428 @@ -/** @file - * @brief Main MAVLink comm protocol routines. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifndef _MAVLINK_PROTOCOL_H_ -#define _MAVLINK_PROTOCOL_H_ - -#include "mavlink_types.h" - -#include "mavlink_checksum.h" - -#ifdef MAVLINK_CHECK_LENGTH -extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; -#endif - -extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; - -extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; -extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; - - -/** - * @brief Initialize the communication stack - * - * This function has to be called before using commParseBuffer() to initialize the different status registers. - * - * @return Will initialize the different buffers and status registers. - */ -static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) -{ - if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) - { - initStatus->ck_a = 0; - initStatus->ck_b = 0; - initStatus->msg_received = 0; - initStatus->buffer_overrun = 0; - initStatus->parse_error = 0; - initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; - initStatus->packet_idx = 0; - initStatus->packet_rx_drop_count = 0; - initStatus->packet_rx_success_count = 0; - initStatus->current_rx_seq = 0; - initStatus->current_tx_seq = 0; - } -} - -static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) -{ - - return &m_mavlink_status[chan]; -} - -/** - * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. - * - * @warning This function implicitely assumes the message is sent over channel zero. - * if the message is sent over a different channel it will reach the receiver - * without error, BUT the sequence number might be wrong due to the wrong - * channel sequence counter. This will result is wrongly reported excessive - * packet loss. Please use @see mavlink_{pack|encode}_headerless and then - * @see mavlink_finalize_message_chan before sending for a correct channel - * assignment. Please note that the mavlink_msg_xxx_pack and encode functions - * assign channel zero as default and thus induce possible loss counter errors.\ - * They have been left to ensure code compatibility. - * - * @see mavlink_finalize_message_chan - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length, usually just the counter incremented while packing the message - */ -static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; -// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte - msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; -} - -/** - * @brief Finalize a MAVLink message with channel assignment - * - * This function calculates the checksum and sets length and aircraft id correctly. - * It assumes that the message id and the payload are already correctly set. This function - * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack - * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. - * - * @param msg Message to finalize - * @param system_id Id of the sending (this) system, 1-127 - * @param length Message length, usually just the counter incremented while packing the message - */ -static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) -{ - // This code part is the same for all messages; - uint16_t checksum; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; -// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); - checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte - msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; -} - -/** - * @brief Pack a message to send it over a serial byte stream - */ -static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) -{ - *(buffer+0) = MAVLINK_STX; ///< Start transmit -// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload - memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header - memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; -// return 0; -} - -/** - * @brief Get the required buffer size for this message - */ -static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) -{ - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; -} - -union checksum_ { - uint16_t s; - uint8_t c[2]; -}; - -static inline void mavlink_start_checksum(mavlink_message_t* msg) -{ - union checksum_ ck; - crc_init(&(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; -} - -static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) -{ - union checksum_ ck; - ck.c[0] = msg->ck_a; - ck.c[1] = msg->ck_b; - crc_accumulate(c, &(ck.s)); - msg->ck_a = ck.c[0]; - msg->ck_b = ck.c[1]; -} - -/** - * This is a convenience function which handles the complete MAVLink parsing. - * the function will parse one byte at a time and return the complete packet once - * it could be successfully decoded. Checksum and other failures will be silently - * ignored. - * - * @param chan ID of the current channel. This allows to parse different channels with this function. - * a channel is not a physical message channel like a serial port, but a logic partition of - * the communication streams in this case. COMM_NB is the limit for the number of channels - * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows - * @param c The char to barse - * - * @param returnMsg NULL if no message could be decoded, the message data else - * @return 0 if no message could be decoded, 1 else - * - * A typical use scenario of this function call is: - * - * @code - * #include // For fixed-width uint8_t type - * - * mavlink_message_t msg; - * int chan = 0; - * - * - * while(serial.bytesAvailable > 0) - * { - * uint8_t byte = serial.getNextByte(); - * if (mavlink_parse_char(chan, byte, &msg)) - * { - * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); - * } - * } - * - * - * @endcode - */ -#ifdef MAVLINK_STATIC_BUFFER -static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -#else -static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) -#endif -{ - // Initializes only once, values keep unchanged after first initialization - mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); - - mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); -#ifdef MAVLINK_CHECK_LENGTH - if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) - status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway - else ; -#endif - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - rxmsg->payload[status->packet_idx++] = c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - mavlink_update_checksum(rxmsg, - MAVLINK_CONST_READ( mavlink_msg_lengths[rxmsg->msgid] ) ); - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: - if (c != rxmsg->ck_a) - { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != rxmsg->ck_b) - {// Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if ( r_message != NULL ) - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - else ; - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; - if (status->msg_received == 1) - { - if ( r_message != NULL ) - return r_message; - else return rxmsg; - } else return NULL; -} - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -// To make MAVLink work on your MCU, define a similar function - -/* - -#include "mavlink_types.h" - -void comm_send_ch(mavlink_channel_t chan, uint8_t ch) -{ - if (chan == MAVLINK_COMM_0) - { - uart0_transmit(ch); - } - if (chan == MAVLINK_COMM_1) - { - uart1_transmit(ch); - } -} - - -static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) -{ - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - comm_send_ch(chan, MAVLINK_STX); - comm_send_ch(chan, msg->len); - comm_send_ch(chan, msg->seq); - comm_send_ch(chan, msg->sysid); - comm_send_ch(chan, msg->compid); - comm_send_ch(chan, msg->msgid); - for(uint16_t i = 0; i < msg->len; i++) - { - comm_send_ch(chan, msg->payload[i]); - } - comm_send_ch(chan, msg->ck_a); - comm_send_ch(chan, msg->ck_b); -} - -static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) -{ - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - for(uint16_t i = 0; i < num; i++) - { - comm_send_ch( chan, mem[i] ); - } -} - */ -static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); -static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); -#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) -#endif - -#endif /* _MAVLINK_PROTOCOL_H_ */ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +extern mavlink_system_t mavlink_system; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Finalize a MAVLink message with channel assignment + * + * This function calculates the checksum and sets length and aircraft id correctly. + * It assumes that the message id and the payload are already correctly set. This function + * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack + * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead. + * + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint8_t key; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit + // memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + // return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + crc_init(&msg->ck); +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + crc_accumulate(c, &msg->ck); +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STATIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + { + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + break; + } else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; +#ifdef MAVLINK_STATIC_BUFFER + if (status->msg_received == 1) + { + if ( r_message != NULL ) + { + return r_message; + } + else + { + return rxmsg; + } + } else return NULL; +#else + if (status->msg_received == 1) + return 1; + else return 0; +#endif +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, uint8_t *mem, uint16_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h old mode 100755 new mode 100644 index 36d8d57c5..77b2e2409 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -1,109 +1,120 @@ -/** @file - * @brief MAVLink comm protocol enumerations / structures / constants. - * @see http://qgroundcontrol.org/mavlink/ - * Edited on Monday, August 8 2011 - */ - -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include "inttypes.h" - -#define MAVLINK_STX 0xD5 ///< Packet start sign -#define MAVLINK_STX_LEN 1 ///< Length of start sign -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length -//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN - -typedef struct __mavlink_system -{ - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message -{ - union - { - uint16_t ck; ///< Checksum high byte - struct - { - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; - }; - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU -} mavlink_message_t; - -typedef struct __mavlink_header -{ - union { - uint16_t ck; ///< Checksum high byte - uint8_t ck_a; ///< Checksum low byte - uint8_t ck_b; ///< Checksum high byte - }; - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload -} mavlink_header_t; - -typedef enum -{ - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_NB, - MAVLINK_COMM_NB_HIGH = 16 -} mavlink_channel_t; - -typedef enum -{ - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status -{ - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#endif /* MAVLINK_TYPES_H_ */ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system +{ + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode + uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode +} mavlink_system_t; + +typedef struct __mavlink_message +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload +} mavlink_header_t; + +typedef enum +{ + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 +} mavlink_channel_t; + +typedef enum +{ + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status +{ + union + { + uint16_t ck; ///< Checksum word + struct + { + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + }; + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 4d4c1e52a..eceea294f 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -995,51 +995,39 @@ 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 - - - The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Attitude roll: -128: -100%, 127: +100% - Attitude pitch: -128: -100%, 127: +100% - Attitude yaw: -128: -100%, 127: +100% - Attitude thrust: -128: -100%, 127: +100% - - - The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Position x: -128: -100%, 127: +100% - Position y: -128: -100%, 127: +100% - Position z: -128: -100%, 127: +100% - Position yaw: -128: -100%, 127: +100% + 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 -- 2.22.0