diff --git a/libuavcan/include/uavcan/global_data_type_registry.hpp b/libuavcan/include/uavcan/global_data_type_registry.hpp index 56d9afa790..8ffe1d55e4 100644 --- a/libuavcan/include/uavcan/global_data_type_registry.hpp +++ b/libuavcan/include/uavcan/global_data_type_registry.hpp @@ -30,6 +30,17 @@ class GlobalDataTypeRegistry : Noncopyable { } }; + struct EntryInsertionComparator + { + const uint16_t id; + EntryInsertionComparator(Entry* dtd) : id(dtd->decriptor.getID()) { } + bool operator()(const Entry* entry) const + { + assert(entry); + return entry->decriptor.getID() > id; + } + }; + typedef LinkedListRoot List; List msgs_; List srvs_; diff --git a/libuavcan/src/global_data_type_registry.cpp b/libuavcan/src/global_data_type_registry.cpp index 4f5e46acef..ce265b37ea 100644 --- a/libuavcan/src/global_data_type_registry.cpp +++ b/libuavcan/src/global_data_type_registry.cpp @@ -63,7 +63,33 @@ bool GlobalDataTypeRegistry::add(Entry* dtd) if (!list) return false; - list->insert(dtd); + { // Collision check + Entry* p = list->get(); + while (p) + { + if (p->decriptor.getID() == dtd->decriptor.getID()) // ID collision + return false; + p = p->getNextListNode(); + } + } + list->insertBefore(dtd, EntryInsertionComparator(dtd)); + +#if UAVCAN_DEBUG + { // Order check + Entry* p = list->get(); + int id = -1; + while (p) + { + if (id >= p->decriptor.getID()) + { + assert(0); + std::abort(); + } + id = p->decriptor.getID(); + p = p->getNextListNode(); + } + } +#endif return true; } diff --git a/libuavcan/test/global_data_type_registry.cpp b/libuavcan/test/global_data_type_registry.cpp index cc9ccb79d4..f8df40ccd1 100644 --- a/libuavcan/test/global_data_type_registry.cpp +++ b/libuavcan/test/global_data_type_registry.cpp @@ -127,6 +127,7 @@ TEST(GlobalDataTypeRegistry, Basic) /* * These types will be necessary for the aggregate signature test */ + ASSERT_FALSE(GlobalDataTypeRegistry::instance().assign(741)); // COLLISION - error ASSERT_TRUE(GlobalDataTypeRegistry::instance().assign(DataTypeC::DefaultDataTypeID)); uavcan::DefaultDataTypeRegistrator reg_DataTypeD;