diff --git a/Tools/msg/px_generate_uorb_topic_helper.py b/Tools/msg/px_generate_uorb_topic_helper.py index 2ee3e1093b..c5a7e221f8 100644 --- a/Tools/msg/px_generate_uorb_topic_helper.py +++ b/Tools/msg/px_generate_uorb_topic_helper.py @@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path): return spec_temp.parsed_fields() +def get_message_fields_str_for_message_hash(msg_fields, search_path): + """ + Get all fields (including for nested types) in the form of: +''' +uint64 timestamp +uint8 esc_count +uint8 esc_online_flags +EscReport[8] esc +uint64 timestamp +uint32 esc_errorcount +int32 esc_rpm +float32 esc_voltage +uint16 failures +int8 esc_power +''' + """ + all_fields_str = '' + for field in msg_fields: + if field.is_header: + continue + + type_name = field.type + # detect embedded types + sl_pos = type_name.find('/') + if sl_pos >= 0: + type_name = type_name[sl_pos + 1:] + + all_fields_str += type_name + ' ' + field.name + '\n' + + if sl_pos >= 0: # nested type, add all nested fields + children_fields = get_children_fields(field.base_type, search_path) + all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path) + + return all_fields_str + + +def hash_32_fnv1a(data: str): + hash_val = 0x811c9dc5 + prime = 0x1000193 + for i in range(len(data)): + value = ord(data[i]) + hash_val = hash_val ^ value + hash_val *= prime + hash_val &= 0xffffffff + return hash_val + + +def get_message_hash(msg_fields, search_path): + """ + Get a 32 bit message hash over all fields + """ + all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path) + return hash_32_fnv1a(all_fields_str) + + def add_padding_bytes(fields, search_path): """ Add padding fields before the embedded types, at the end and calculate the diff --git a/Tools/msg/templates/uorb/msg.cpp.em b/Tools/msg/templates/uorb/msg.cpp.em index ad13a83e40..b77c013b91 100644 --- a/Tools/msg/templates/uorb/msg.cpp.em +++ b/Tools/msg/templates/uorb/msg.cpp.em @@ -58,6 +58,7 @@ from px_generate_uorb_topic_helper import * # this is in Tools/ uorb_struct = '%s_s'%name_snake_case +message_hash = get_message_hash(spec.parsed_fields(), search_path) sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True) struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) }@ @@ -74,7 +75,7 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path) @[for topic in topics]@ static_assert(static_cast(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch"); -ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast(ORB_ID::@topic)); +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast(ORB_ID::@topic)); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 646e470a90..826f1cb6ab 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -133,6 +133,8 @@ set(msg_files ManualControlSwitches.msg MavlinkLog.msg MavlinkTunnel.msg + MessageFormatRequest.msg + MessageFormatResponse.msg Mission.msg MissionResult.msg MountOrientation.msg diff --git a/msg/MessageFormatRequest.msg b/msg/MessageFormatRequest.msg new file mode 100644 index 0000000000..c852a6f005 --- /dev/null +++ b/msg/MessageFormatRequest.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +# Request to PX4 to get the hash of a message, to check for message compatibility + +uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes. + +uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp + +char[50] topic_name # E.g. /fmu/in/vehicle_command + diff --git a/msg/MessageFormatResponse.msg b/msg/MessageFormatResponse.msg new file mode 100644 index 0000000000..41ee962749 --- /dev/null +++ b/msg/MessageFormatResponse.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +# Response from PX4 with the format of a message + +uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp + +char[50] topic_name # E.g. /fmu/in/vehicle_command + +bool success +uint32 message_hash # hash over all message fields + diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 7abbd7a568..fc06a2da3c 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -51,6 +51,7 @@ struct orb_metadata { const char *o_name; /**< unique object name */ const uint16_t o_size; /**< object size */ const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ + uint32_t message_hash; /**< Hash over all fields for message compatibility checks */ orb_id_size_t o_id; /**< ORB_ID enum */ }; @@ -99,13 +100,15 @@ typedef const struct orb_metadata *orb_id_t; * @param _name The name of the topic. * @param _struct The structure the topic provides. * @param _size_no_padding Struct size w/o padding at the end + * @param _message_hash 32 bit message hash over all fields * @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status */ -#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum) \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \ const struct orb_metadata __orb_##_name = { \ #_name, \ sizeof(_struct), \ _size_no_padding, \ + _message_hash, \ _orb_id_enum \ }; struct hack diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 0f819bb3be..94ca33ee34 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -23,6 +23,9 @@ publications: - topic: /fmu/out/manual_control_setpoint type: px4_msgs::msg::ManualControlSetpoint + - topic: /fmu/out/message_format_response + type: px4_msgs::msg::MessageFormatResponse + - topic: /fmu/out/position_setpoint_triplet type: px4_msgs::msg::PositionSetpointTriplet @@ -75,6 +78,9 @@ subscriptions: - topic: /fmu/in/arming_check_reply type: px4_msgs::msg::ArmingCheckReply + - topic: /fmu/in/message_format_request + type: px4_msgs::msg::MessageFormatRequest + - topic: /fmu/in/mode_completed type: px4_msgs::msg::ModeCompleted diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 4890582464..f76c15011b 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -188,6 +188,67 @@ UxrceddsClient::~UxrceddsClient() } } +static void fillMessageFormatResponse(const message_format_request_s &message_format_request, + message_format_response_s &message_format_response) +{ + message_format_response.protocol_version = message_format_request_s::LATEST_PROTOCOL_VERSION; + message_format_response.success = false; + + if (message_format_request.protocol_version == message_format_request_s::LATEST_PROTOCOL_VERSION) { + static_assert(sizeof(message_format_request.topic_name) == sizeof(message_format_response.topic_name), "size mismatch"); + memcpy(message_format_response.topic_name, message_format_request.topic_name, + sizeof(message_format_response.topic_name)); + + // Get the topic name by searching for the last '/' + int idx_last_slash = -1; + bool found_null = false; + + for (int i = 0; i < (int)sizeof(message_format_request.topic_name); ++i) { + if (message_format_request.topic_name[i] == 0) { + found_null = true; + break; + } + + if (message_format_request.topic_name[i] == '/') { + idx_last_slash = i; + } + } + + if (found_null && idx_last_slash != -1) { + const char *topic_name = message_format_request.topic_name + idx_last_slash + 1; + // Find the format + const orb_metadata *const *topics = orb_get_topics(); + const orb_metadata *topic_meta{nullptr}; + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strcmp(topic_name, topics[i]->o_name) == 0) { + topic_meta = topics[i]; + break; + } + } + + if (topic_meta) { + message_format_response.message_hash = topic_meta->message_hash; + // The topic type is already checked by DDS + message_format_response.success = true; + } + } + } + + message_format_response.timestamp = hrt_absolute_time(); +} + +void UxrceddsClient::handleMessageFormatRequest() +{ + message_format_request_s message_format_request; + + if (_message_format_request_sub.update(&message_format_request)) { + message_format_response_s message_format_response; + fillMessageFormatResponse(message_format_request, message_format_response); + _message_format_response_pub.publish(message_format_response); + } +} + void UxrceddsClient::run() { if (!_comm) { @@ -405,6 +466,8 @@ void UxrceddsClient::run() } } + handleMessageFormatRequest(); + // Check for a ping response /* PONG_IN_SESSION_STATUS */ if (session.on_pong_flag == 1) { diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index 55f6dfb9e4..4ba16e0f50 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -38,6 +38,10 @@ #include +#include +#include +#include + #include #include "srv_base.h" @@ -106,6 +110,11 @@ public: private: int setBaudrate(int fd, unsigned baud); + void handleMessageFormatRequest(); + + uORB::Publication _message_format_response_pub{ORB_ID(message_format_response)}; + uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)}; + const bool _localhost_only; const bool _custom_participant; const char *_client_namespace;