mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 14:19:57 +08:00
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>
This commit is contained in:
1 parent
1c8874d41a
commit
8fc6cf05ae
17 files changed
+628
-1
No files matched your search
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -44,6 +44,7 @@
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_boardconfig.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
@@ -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()
|
||||
@@ -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<uint8_t>(_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<uint8_t>(open_drone_id_translations::odidTypeForMavType(_vehicle_status.get().system_type));
|
||||
|
||||
@@ -54,6 +54,9 @@
|
||||
#include <dronecan/remoteid/System.hpp>
|
||||
#include <dronecan/remoteid/ArmStatus.hpp>
|
||||
#include <dronecan/remoteid/OperatorID.hpp>
|
||||
#if defined(CONFIG_BOARD_HAS_RID_EEPROM)
|
||||
#include <lib/open_drone_id/odid_bid_storage.hpp>
|
||||
#endif
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
@@ -105,5 +108,16 @@ private:
|
||||
using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *,
|
||||
void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &) >;
|
||||
|
||||
#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<px4::params::ODID_BID_SRC>) _param_odid_bid_src
|
||||
)
|
||||
#endif
|
||||
uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusBinder> _uavcan_sub_arm_status;
|
||||
};
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
@@ -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 <fcntl.h>
|
||||
#include <stddef.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <nuttx/crc32.h>
|
||||
#include <px4_platform_common/px4_manifest.h>
|
||||
#else
|
||||
#include <crc32.h>
|
||||
#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<const uint8_t *>(&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<const uint8_t *>(&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<const uint8_t *>(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<off_t>(sizeof(record.magic))
|
||||
&& writeAll(fd, reinterpret_cast<const uint8_t *>(&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
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
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
|
||||
@@ -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
|
||||
@@ -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)
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
|
||||
#include <math.h>
|
||||
#include <poll.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef CONFIG_NET
|
||||
#include <net/if.h>
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -56,6 +56,9 @@
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#if defined(CONFIG_BOARD_HAS_RID_EEPROM)
|
||||
#include <lib/open_drone_id/odid_bid_storage.hpp>
|
||||
#endif
|
||||
#include <lib/systemlib/mavlink_log.h>
|
||||
|
||||
#include <lib/failure_injection/FailureInjection.hpp>
|
||||
@@ -496,6 +499,9 @@ private:
|
||||
hrt_abstime _heartbeat_component_uart_bridge{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
#if defined(CONFIG_BOARD_HAS_RID_EEPROM)
|
||||
(ParamInt<px4::params::ODID_BID_SRC>) _param_odid_bid_src,
|
||||
#endif
|
||||
(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
|
||||
(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
|
||||
(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
|
||||
@@ -504,6 +510,11 @@ private:
|
||||
(ParamFloat<px4::params::BAT1_V_EMPTY>) _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;
|
||||
|
||||
Reference in new issue
Block a user