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;