From 8fc6cf05aecfc853690755bfaa17bb956a14b52a Mon Sep 17 00:00:00 2001 From: msli-dev <97445659+tustin-dev@users.noreply.github.com> Date: Mon, 19 Oct 2026 22:30:20 +0800 Subject: [PATCH] feat(open_drone_id): add locked Basic ID storage and Mach RID target (#29076) * feat(open_drone_id): add locked Basic ID storage and Mach RID target Persist the first serial-number Basic ID in MTD storage so DroneCAN can reuse the identity after reboot without falling back to the board GUID in locked mode. Provide a separate Mach RID build with EEPROM storage and locked-mode defaults while keeping EEPROM registration out of the default build. Signed-off-by: msli-dev <747640013@qq.com> Assisted-by: Codex * fix(open_drone_id): gate storage and preserve legacy forwarding Keep the optional EEPROM identity library, parameters and per-instance state out of builds that do not enable CONFIG_BOARD_HAS_RID_EEPROM. Use the same option for Mach EEPROM registration and module dependencies. Restore the original Basic ID forwarding path when locking is disabled, including builds without identity storage. Limit persistent identity resolution to the enabled locked mode. Signed-off-by: msli-dev <747640013@qq.com> Assisted-by: Codex --------- Signed-off-by: msli-dev <747640013@qq.com> --- Kconfig | 9 + boards/tustin/mach/init/rc.rid | 3 + boards/tustin/mach/rid.px4board | 4 + boards/tustin/mach/src/board_config.h | 5 +- boards/tustin/mach/src/mtd.cpp | 10 + src/drivers/uavcan/CMakeLists.txt | 4 + src/drivers/uavcan/remoteid.cpp | 31 ++ src/drivers/uavcan/remoteid.hpp | 14 + src/lib/CMakeLists.txt | 1 + src/lib/open_drone_id/CMakeLists.txt | 40 +++ src/lib/open_drone_id/odid_bid_storage.cpp | 339 +++++++++++++++++++++ src/lib/open_drone_id/odid_bid_storage.hpp | 94 ++++++ src/lib/open_drone_id/odid_params.yaml | 17 ++ src/lib/parameters/CMakeLists.txt | 5 + src/modules/mavlink/CMakeLists.txt | 4 + src/modules/mavlink/mavlink_receiver.cpp | 38 +++ src/modules/mavlink/mavlink_receiver.h | 11 + 17 files changed, 628 insertions(+), 1 deletion(-) create mode 100644 boards/tustin/mach/init/rc.rid create mode 100644 boards/tustin/mach/rid.px4board create mode 100644 src/lib/open_drone_id/CMakeLists.txt create mode 100644 src/lib/open_drone_id/odid_bid_storage.cpp create mode 100644 src/lib/open_drone_id/odid_bid_storage.hpp create mode 100644 src/lib/open_drone_id/odid_params.yaml diff --git a/Kconfig b/Kconfig index ee2a5af4694..9c678825f39 100644 --- a/Kconfig +++ b/Kconfig @@ -138,6 +138,15 @@ config BOARD_ETHERNET help flag to indicate that ethernet is enabled +config BOARD_HAS_RID_EEPROM + bool "Remote ID identity EEPROM" + default n + depends on PLATFORM_NUTTX && SYSTEMCMDS_MTD + help + Enable persistent Open Drone ID Basic ID storage on boards with + an MTD_ID EEPROM partition. Includes the ODID_BID_SRC parameter + and the MAVLink/DroneCAN locked identity paths. + config BOARD_CRYPTO bool "Crypto support" help diff --git a/boards/tustin/mach/init/rc.rid b/boards/tustin/mach/init/rc.rid new file mode 100644 index 00000000000..821120e2822 --- /dev/null +++ b/boards/tustin/mach/init/rc.rid @@ -0,0 +1,3 @@ +#!/bin/sh +# Use the first received serial-number Basic ID stored in the RID EEPROM. +param set-default ODID_BID_SRC 1 diff --git a/boards/tustin/mach/rid.px4board b/boards/tustin/mach/rid.px4board new file mode 100644 index 00000000000..002b8d666ca --- /dev/null +++ b/boards/tustin/mach/rid.px4board @@ -0,0 +1,4 @@ +# Inherit default.px4board and enable the onboard Remote ID EEPROM. +CONFIG_BOARD_HAS_RID_EEPROM=y +CONFIG_BOARD_ADDITIONAL_INIT="rc.rid" +CONFIG_UAVCAN_REMOTEID_CONTROLLER=y diff --git a/boards/tustin/mach/src/board_config.h b/boards/tustin/mach/src/board_config.h index 7ad32f142fb..5682f199299 100644 --- a/boards/tustin/mach/src/board_config.h +++ b/boards/tustin/mach/src/board_config.h @@ -44,6 +44,7 @@ ****************************************************************************************************/ #include +#include #include #include @@ -155,9 +156,11 @@ #define BOARD_NUM_IO_TIMERS 3 -/* Onboard 24LC64 EEPROM on I2C2, address 0x51. */ +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) +/* Onboard 24LC64 EEPROM on I2C2, address 0x51 (RID variant only). */ #define PX4_I2C_BUS_MTD 2 #define BOARD_MTD_NUM_EEPROM 1 +#endif __BEGIN_DECLS diff --git a/boards/tustin/mach/src/mtd.cpp b/boards/tustin/mach/src/mtd.cpp index 028471d32a3..fea6292ebc7 100644 --- a/boards/tustin/mach/src/mtd.cpp +++ b/boards/tustin/mach/src/mtd.cpp @@ -42,10 +42,12 @@ static const px4_mft_device_t fram_device = { .devid = SPIDEV_FLASH(0), }; +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) static const px4_mft_device_t rid_eeprom_device = { .bus_type = px4_mft_device_t::I2C, .devid = PX4_MK_I2C_DEVID(2, 0x51), }; +#endif static const px4_mtd_entry_t fram = { .device = &fram_device, @@ -59,6 +61,7 @@ static const px4_mtd_entry_t fram = { }, }; +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) static const px4_mtd_entry_t rid_eeprom = { .device = &rid_eeprom_device, .npart = 1, @@ -71,12 +74,19 @@ static const px4_mtd_entry_t rid_eeprom = { }, }, }; +#endif static const px4_mtd_manifest_t board_mtd_config = { +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) .nconfigs = 2, +#else + .nconfigs = 1, +#endif .entries = { &fram, +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) &rid_eeprom, +#endif }, }; diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index e9d205c7a13..f70e0866adc 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -213,3 +213,7 @@ if(CONFIG_UAVCAN_REMOTEID_CONTROLLER) # the MAVLink headers, generated by the mavlink module add_dependencies(drivers__uavcan mavlink_c_generate) endif() + +if(CONFIG_BOARD_HAS_RID_EEPROM) + target_link_libraries(drivers__uavcan PRIVATE open_drone_id) +endif() diff --git a/src/drivers/uavcan/remoteid.cpp b/src/drivers/uavcan/remoteid.cpp index 87c236707c5..869d564a0fe 100644 --- a/src/drivers/uavcan/remoteid.cpp +++ b/src/drivers/uavcan/remoteid.cpp @@ -53,6 +53,11 @@ UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) : int UavcanRemoteIDController::init() { +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + // Cache the source of the basic ID + _odid_bid_src = static_cast(_param_odid_bid_src.get()); +#endif + // Setup timer and call back function for periodic updates _timer.setCallback(TimerCbBinder(this, &UavcanRemoteIDController::periodic_update)); _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); @@ -98,8 +103,34 @@ void UavcanRemoteIDController::send_basic_id() msg.uas_id.push_back(basic_id.uas_id[i]); } +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + + } else if (_odid_bid_src == open_drone_id::UAS_ID_MODE_LOCK_FIRST_BASIC_ID) { + return; +#endif } +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + + } else if (_odid_bid_src == open_drone_id::UAS_ID_MODE_LOCK_FIRST_BASIC_ID) { + if (!_odid_bid_lock_loaded) { + _odid_bid_lock_info = open_drone_id::BidStorage::get(); + _odid_bid_lock_loaded = true; + } + + if (!_odid_bid_lock_info.locked()) { + return; + } + + msg.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER; + msg.ua_type = _odid_bid_lock_info.id.ua_type; + + for (unsigned i = 0; i < open_drone_id::UAS_ID_SIZE; ++i) { + msg.uas_id.push_back(_odid_bid_lock_info.id.uas_id[i]); + } + +#endif + } else { msg.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER; msg.ua_type = static_cast(open_drone_id_translations::odidTypeForMavType(_vehicle_status.get().system_type)); diff --git a/src/drivers/uavcan/remoteid.hpp b/src/drivers/uavcan/remoteid.hpp index 45408f0bc1b..78b63a42c76 100644 --- a/src/drivers/uavcan/remoteid.hpp +++ b/src/drivers/uavcan/remoteid.hpp @@ -54,6 +54,9 @@ #include #include #include +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) +#include +#endif #include @@ -105,5 +108,16 @@ private: using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *, void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure &) >; +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + // Cached configuration (set during init, requires reboot to change) + uint8_t _odid_bid_src{0}; + + open_drone_id::BidLockInfo _odid_bid_lock_info{}; + bool _odid_bid_lock_loaded{false}; + + DEFINE_PARAMETERS( + (ParamInt) _param_odid_bid_src + ) +#endif uavcan::Subscriber _uavcan_sub_arm_status; }; diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 97bfca96710..cfcf91889c8 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -65,6 +65,7 @@ add_subdirectory(mathlib EXCLUDE_FROM_ALL) add_subdirectory(mixer_module EXCLUDE_FROM_ALL) add_subdirectory(motion_planning EXCLUDE_FROM_ALL) add_subdirectory(npfg EXCLUDE_FROM_ALL) +add_subdirectory(open_drone_id EXCLUDE_FROM_ALL) add_subdirectory(perf EXCLUDE_FROM_ALL) add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL) add_subdirectory(pid EXCLUDE_FROM_ALL) diff --git a/src/lib/open_drone_id/CMakeLists.txt b/src/lib/open_drone_id/CMakeLists.txt new file mode 100644 index 00000000000..ad7d6b57492 --- /dev/null +++ b/src/lib/open_drone_id/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2026 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +if(CONFIG_BOARD_HAS_RID_EEPROM) + px4_add_library(open_drone_id + odid_bid_storage.cpp + ) + + set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/odid_params.yaml) +endif() diff --git a/src/lib/open_drone_id/odid_bid_storage.cpp b/src/lib/open_drone_id/odid_bid_storage.cpp new file mode 100644 index 00000000000..9d3103513fe --- /dev/null +++ b/src/lib/open_drone_id/odid_bid_storage.cpp @@ -0,0 +1,339 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "odid_bid_storage.hpp" + +#include +#include +#include +#include + +#ifdef __PX4_NUTTX +#include +#include +#else +#include +#endif + +namespace open_drone_id +{ +namespace +{ + +static constexpr uint32_t ODID_UASID_MAGIC = 0x4449444f; // "ODID" +static constexpr uint8_t ODID_UASID_RECORD_VERSION = 1; + +struct __attribute__((packed)) UasIdRecord { + uint32_t magic; + uint8_t version; + uint8_t id_type; + uint8_t ua_type; + uint8_t reserved; + uint8_t uas_id[UAS_ID_SIZE]; + uint32_t crc; +}; + +class BidStorageBackend +{ +public: + BidLockInfo get(); + BidStoreResult checkOrStore(uint8_t ua_type, const uint8_t uas_id[UAS_ID_SIZE]); + +private: + static bool uasIdIsEmpty(const uint8_t uas_id[UAS_ID_SIZE]) + { + for (unsigned i = 0; i < UAS_ID_SIZE; ++i) { + if (uas_id[i] != 0) { + return false; + } + } + + return true; + } + + static uint32_t recordCrc(const UasIdRecord &record) + { + return crc32(reinterpret_cast(&record.version), + offsetof(UasIdRecord, crc) - offsetof(UasIdRecord, version)); + } + + static bool recordValid(const UasIdRecord &record) + { + return record.magic == ODID_UASID_MAGIC + && record.version == ODID_UASID_RECORD_VERSION + && record.id_type == UAS_ID_TYPE_SERIAL_NUMBER + && !uasIdIsEmpty(record.uas_id) + && record.crc == recordCrc(record); + } + + // Check if the record is all 0xff, which is the erased state of flash. + // This is needed to distinguish between an unwritten record and a record that was written with all bytes set to 0 (which would be invalid but not unwritten). + static bool recordBlank(const UasIdRecord &record) + { + const uint8_t *bytes = reinterpret_cast(&record); + + for (unsigned i = 0; i < sizeof(record); ++i) { + if (bytes[i] != 0xff) { + return false; + } + } + + return true; + } + + static bool recordMatches(const UasIdRecord &record, const uint8_t uas_id[UAS_ID_SIZE]) + { + return memcmp(record.uas_id, uas_id, UAS_ID_SIZE) == 0; + } + + // Write the entire buffer to the file descriptor, handling short writes. + static bool writeAll(int fd, const void *buffer, size_t size) + { + const uint8_t *bytes = static_cast(buffer); + + while (size > 0) { + const ssize_t written = write(fd, bytes, size); + + if (written <= 0) { + return false; + } + + bytes += written; + size -= written; + } + + return true; + } + +}; + +static_assert(sizeof(UasIdRecord) <= 256, "OpenDroneID UAS ID record must fit in /fs/mtd_id"); + +// The UAS ID storage is only supported on PX4 targets with NuttX and an MTD partition named "ID". +// On other platforms, the get() method will return StorageUnavailable and the checkOrStore() method will return Unsupported. +BidLockInfo BidStorageBackend::get() +{ +#if defined(__PX4_NUTTX) + const char *path = nullptr; + + if (px4_mtd_query("MTD_ID", nullptr, &path) != 0 || path == nullptr) { + return {BidLockState::StorageUnavailable, {}}; + } + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + return {BidLockState::StorageUnavailable, {}}; + } + + UasIdRecord record{}; + + if (read(fd, &record, sizeof(record)) != sizeof(record)) { + close(fd); + return {BidLockState::StorageUnavailable, {}}; + } + + close(fd); + + if (recordValid(record)) { + BidLockInfo info{BidLockState::Locked, {}}; + info.id.ua_type = record.ua_type; + memcpy(info.id.uas_id, record.uas_id, UAS_ID_SIZE); + return info; + } + + if (recordBlank(record)) { + return {BidLockState::Empty, {}}; + } + + return {BidLockState::InvalidRecord, {}}; +#else + return {BidLockState::Unsupported, {}}; +#endif +} + +// Check if the given UAS ID matches the stored one, or store it if the storage is empty. +// The storage is write-once, so once a UAS ID is stored it cannot be changed (except by erasing the flash). +BidStoreResult BidStorageBackend::checkOrStore(uint8_t ua_type, const uint8_t uas_id[UAS_ID_SIZE]) +{ + if (uasIdIsEmpty(uas_id)) { + return BidStoreResult::InvalidBasicId; + } + +#if defined(__PX4_NUTTX) + const char *path = nullptr; + + if (px4_mtd_query("MTD_ID", nullptr, &path) != 0 || path == nullptr) { + return BidStoreResult::StorageUnavailable; + } + + int fd = open(path, O_RDWR); + + if (fd < 0) { + return BidStoreResult::StorageUnavailable; + } + + UasIdRecord existing{}; + + if (read(fd, &existing, sizeof(existing)) != sizeof(existing)) { + close(fd); + return BidStoreResult::StorageUnavailable; + } + + if (recordValid(existing)) { + const BidStoreResult result = recordMatches(existing, uas_id) + ? BidStoreResult::AlreadyLocked + : BidStoreResult::AlreadyLockedDifferent; + close(fd); + return result; + } + + if (!recordBlank(existing)) { + close(fd); + return BidStoreResult::InvalidRecord; + } + + UasIdRecord record{}; + record.magic = ODID_UASID_MAGIC; + record.version = ODID_UASID_RECORD_VERSION; + record.id_type = UAS_ID_TYPE_SERIAL_NUMBER; + record.ua_type = ua_type; + memcpy(record.uas_id, uas_id, UAS_ID_SIZE); + record.crc = recordCrc(record); + + const uint32_t magic = record.magic; + record.magic = 0xffffffff; + + const bool written = lseek(fd, sizeof(record.magic), SEEK_SET) == static_cast(sizeof(record.magic)) + && writeAll(fd, reinterpret_cast(&record) + sizeof(record.magic), + sizeof(record) - sizeof(record.magic)) + && fsync(fd) == 0 + && lseek(fd, 0, SEEK_SET) == 0 + && writeAll(fd, &magic, sizeof(magic)) + && fsync(fd) == 0; + + close(fd); + + return written ? BidStoreResult::Stored : BidStoreResult::WriteError; +#else + return BidStoreResult::Unsupported; +#endif +} + +} // namespace + +// Public interface that uses the backend implementation. +BidLockInfo BidStorage::get() +{ + BidStorageBackend storage; + return storage.get(); +} + +BidStoreResult BidStorage::checkOrStore(uint8_t ua_type, const uint8_t uas_id[UAS_ID_SIZE]) +{ + BidStorageBackend storage; + return storage.checkOrStore(ua_type, uas_id); +} + +// +BidStoreResult BidLock::resolve(uint8_t id_type, uint8_t ua_type, const uint8_t uas_id[UAS_ID_SIZE], + UasId &locked_id) +{ + if (id_type != UAS_ID_TYPE_SERIAL_NUMBER) { + return BidStoreResult::InvalidBasicId; + } + + if (!_loaded) { + _info = BidStorage::get(); + _loaded = true; + } + + if (_info.locked()) { + locked_id = _info.id; + return BidStoreResult::AlreadyLocked; + } + + if (_info.state != BidLockState::Empty) { + switch (_info.state) { + case BidLockState::InvalidRecord: + return BidStoreResult::InvalidRecord; + + case BidLockState::StorageUnavailable: + return BidStoreResult::StorageUnavailable; + + case BidLockState::WriteError: + return BidStoreResult::WriteError; + + case BidLockState::Unsupported: + return BidStoreResult::Unsupported; + + case BidLockState::Empty: + case BidLockState::Locked: + break; + } + } + + const BidStoreResult result = BidStorage::checkOrStore(ua_type, uas_id); + + switch (result) { + case BidStoreResult::Stored: + _info.state = BidLockState::Locked; + _info.id.ua_type = ua_type; + memcpy(_info.id.uas_id, uas_id, UAS_ID_SIZE); + locked_id = _info.id; + return result; + + case BidStoreResult::AlreadyLocked: + case BidStoreResult::AlreadyLockedDifferent: { + _info = BidStorage::get(); + + if (_info.locked()) { + locked_id = _info.id; + return BidStoreResult::AlreadyLocked; + } + + return BidStoreResult::InvalidRecord; + } + + case BidStoreResult::InvalidBasicId: + case BidStoreResult::StorageUnavailable: + case BidStoreResult::InvalidRecord: + case BidStoreResult::WriteError: + case BidStoreResult::Unsupported: + return result; + } + + return BidStoreResult::InvalidRecord; +} + +} // namespace open_drone_id diff --git a/src/lib/open_drone_id/odid_bid_storage.hpp b/src/lib/open_drone_id/odid_bid_storage.hpp new file mode 100644 index 00000000000..b513a0a8dcb --- /dev/null +++ b/src/lib/open_drone_id/odid_bid_storage.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2026 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +namespace open_drone_id +{ + +static constexpr uint8_t UAS_ID_SIZE = 20; +static constexpr uint8_t UAS_ID_TYPE_SERIAL_NUMBER = 1; +static constexpr int32_t UAS_ID_MODE_LOCK_FIRST_BASIC_ID = 1; + +struct UasId { + uint8_t ua_type{0}; + uint8_t uas_id[UAS_ID_SIZE] {}; +}; + +enum class BidLockState { + Empty, + Locked, + InvalidRecord, + StorageUnavailable, + WriteError, + Unsupported, +}; + +struct BidLockInfo { + BidLockState state{BidLockState::Unsupported}; + UasId id{}; + + bool locked() const { return state == BidLockState::Locked; } +}; + +enum class BidStoreResult { + Stored, + AlreadyLocked, + AlreadyLockedDifferent, + InvalidBasicId, + StorageUnavailable, + InvalidRecord, + WriteError, + Unsupported, +}; + +class BidStorage +{ +public: + static BidLockInfo get(); + static BidStoreResult checkOrStore(uint8_t ua_type, const uint8_t uas_id[UAS_ID_SIZE]); +}; + +class BidLock +{ +public: + BidStoreResult resolve(uint8_t id_type, uint8_t ua_type, const uint8_t uas_id[UAS_ID_SIZE], UasId &locked_id); + +private: + BidLockInfo _info{}; + bool _loaded{false}; +}; + +} // namespace open_drone_id diff --git a/src/lib/open_drone_id/odid_params.yaml b/src/lib/open_drone_id/odid_params.yaml new file mode 100644 index 00000000000..55bbc3d0b63 --- /dev/null +++ b/src/lib/open_drone_id/odid_params.yaml @@ -0,0 +1,17 @@ +module_name: open_drone_id +parameters: +- group: Open Drone ID + definitions: + ODID_BID_SRC: + description: + short: Open Drone ID UAS ID source + long: |- + Selects how the Open Drone ID UAS ID is sourced. + + In locked mode, the first received Basic ID UAS ID is expected to be stored persistently and reused on subsequent boots. + type: enum + values: + 0: PX4 board GUID legacy fallback + 1: Lock first received Basic ID + default: 0 + reboot_required: true diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index a62ec94ae1c..feb967605db 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -60,6 +60,11 @@ if(DISABLE_PARAMS_MODULE_SCOPING) list(REMOVE_DUPLICATES module_config_files) endif() +# Keep the opt-in identity parameter out of builds that scan all module YAMLs. +if(NOT CONFIG_BOARD_HAS_RID_EEPROM) + list(REMOVE_ITEM module_config_files ${PX4_SOURCE_DIR}/src/lib/open_drone_id/odid_params.yaml) +endif() + set(generated_params_dir ${PX4_BINARY_DIR}/generated_params) set(generated_serial_params_file ${generated_params_dir}/serial_params.c) set(generated_module_params_file ${generated_params_dir}/module_params.c) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 4ebe382be60..0fe32bbbf33 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -147,6 +147,10 @@ px4_add_module( UNITY_BUILD ) +if(CONFIG_BOARD_HAS_RID_EEPROM) + target_link_libraries(modules__mavlink PRIVATE open_drone_id) +endif() + if(CONFIG_DRIVERS_SERIALPASSTHROUGH) target_link_libraries(modules__mavlink PRIVATE drivers__serialpassthrough) endif() diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6b35af722a6..49266153c0a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -46,6 +46,7 @@ #include #include +#include #ifdef CONFIG_NET #include @@ -3790,6 +3791,43 @@ void MavlinkReceiver::handle_message_open_drone_id_basic_id(mavlink_message_t *m odid_basic_id.ua_type = odid_module.ua_type; memcpy(odid_basic_id.uas_id, odid_module.uas_id, sizeof(odid_basic_id.uas_id)); +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + + if (_param_odid_bid_src.get() == open_drone_id::UAS_ID_MODE_LOCK_FIRST_BASIC_ID) { + open_drone_id::UasId locked_id{}; + const open_drone_id::BidStoreResult result = _odid_bid_lock.resolve(odid_module.id_type, odid_module.ua_type, + odid_module.uas_id, locked_id); + + switch (result) { + case open_drone_id::BidStoreResult::Stored: + mavlink_log_info(&_mavlink_log_pub, "OpenDroneID BID locked"); + break; + + case open_drone_id::BidStoreResult::AlreadyLocked: + case open_drone_id::BidStoreResult::AlreadyLockedDifferent: + break; + + case open_drone_id::BidStoreResult::InvalidBasicId: + return; + + case open_drone_id::BidStoreResult::StorageUnavailable: + case open_drone_id::BidStoreResult::InvalidRecord: + case open_drone_id::BidStoreResult::WriteError: + case open_drone_id::BidStoreResult::Unsupported: + if (!_odid_bid_lock_warning_reported) { + mavlink_log_warning(&_mavlink_log_pub, "OpenDroneID BID lock unavailable"); + _odid_bid_lock_warning_reported = true; + } + + return; + } + + odid_basic_id.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER; + odid_basic_id.ua_type = locked_id.ua_type; + memcpy(odid_basic_id.uas_id, locked_id.uas_id, sizeof(odid_basic_id.uas_id)); + } + +#endif _open_drone_id_basic_id_pub.publish(odid_basic_id); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 63f5dd7b358..d5a474fd12c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -56,6 +56,9 @@ #include #include #include +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) +#include +#endif #include #include @@ -496,6 +499,9 @@ private: hrt_abstime _heartbeat_component_uart_bridge{0}; DEFINE_PARAMETERS( +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + (ParamInt) _param_odid_bid_src, +#endif (ParamFloat) _param_bat_crit_thr, (ParamFloat) _param_bat_emergen_thr, (ParamFloat) _param_bat_low_thr, @@ -504,6 +510,11 @@ private: (ParamFloat) _param_bat_v_empty ); +#if defined(CONFIG_BOARD_HAS_RID_EEPROM) + open_drone_id::BidLock _odid_bid_lock {}; + bool _odid_bid_lock_warning_reported{false}; +#endif + // Disallow copy construction and move assignment. MavlinkReceiver(const MavlinkReceiver &) = delete; MavlinkReceiver operator=(const MavlinkReceiver &) = delete;