AP_OpenDroneID: add support for persistent storage of UAS ID

This commit is contained in:
bugobliterator 2023-05-25 17:55:14 +10:00 committed by Randy Mackay
parent 55504e7fcc
commit b05df95c3d
2 changed files with 117 additions and 3 deletions

View File

@ -38,6 +38,8 @@
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Parachute/AP_Parachute.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <stdio.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL &hal;
@ -64,7 +66,7 @@ const AP_Param::GroupInfo AP_OpenDroneID::var_info[] = {
// @Param: OPTIONS
// @DisplayName: OpenDroneID options
// @Description: Options for OpenDroneID subsystem
// @Bitmask: 0:EnforceArming, 1:AllowNonGPSPosition
// @Bitmask: 0:EnforceArming, 1:AllowNonGPSPosition, 2:LockUASIDOnFirstBasicIDRx, 3:UseChipIDAsBasicID
AP_GROUPINFO("OPTIONS", 4, AP_OpenDroneID, _options, 0),
// @Param: BARO_ACC
@ -77,6 +79,13 @@ const AP_Param::GroupInfo AP_OpenDroneID::var_info[] = {
AP_GROUPEND
};
#if defined(OPENDRONEID_UA_TYPE)
// ensure the type is within the allowed range
#if OPENDRONEID_UA_TYPE < 0 || OPENDRONEID_UA_TYPE > 15
#error "OPENDRONEID_UA_TYPE must be between 0 and 15"
#endif
#endif
// constructor
AP_OpenDroneID::AP_OpenDroneID()
{
@ -95,7 +104,80 @@ void AP_OpenDroneID::init()
return;
}
load_UAS_ID_from_persistent_memory();
_chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
_initialised = true;
}
void AP_OpenDroneID::load_UAS_ID_from_persistent_memory()
{
id_len = sizeof(id_str);
size_t id_type_len = sizeof(id_type);
size_t ua_type_len = sizeof(ua_type);
if (hal.util->get_persistent_param_by_name("DID_UAS_ID", id_str, id_len) &&
hal.util->get_persistent_param_by_name("DID_UAS_ID_TYPE", id_type, id_type_len) &&
hal.util->get_persistent_param_by_name("DID_UA_TYPE", ua_type, ua_type_len)) {
if (id_len && id_type_len && ua_type_len) {
_options.set_and_save(_options.get() & ~LockUASIDOnFirstBasicIDRx);
_options.notify();
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OpenDroneID: Locked UAS_ID: %s", id_str);
}
} else {
id_len = 0;
}
}
void AP_OpenDroneID::set_basic_id() {
if (pkt_basic_id.id_type != MAV_ODID_ID_TYPE_NONE) {
return;
}
if (id_len == 0) {
load_UAS_ID_from_persistent_memory();
}
#if defined(OPENDRONEID_UA_MFR_CODE)
if ((_options & UseChipIDAsBasicID) && (id_len == 0)) {
float val;
// prepare basic id pkt
AP_Param::get("SYSID_THISMAV", val);
pkt_basic_id.target_system = val;
pkt_basic_id.target_component = MAV_COMP_ID_ODID_TXRX_1;
pkt_basic_id.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER;
pkt_basic_id.ua_type = OPENDRONEID_UA_TYPE;
uint8_t sys_id[12];
uint8_t len = 12;
hal.util->get_system_id_unformatted(sys_id, len);
char buffer[21];
// create a unique id based on the system id, using
snprintf(buffer, sizeof(buffer), "%sE%x%x%x%x%x%x%x",
OPENDRONEID_UA_MFR_CODE,
sys_id[0], sys_id[1], sys_id[2],
sys_id[4],
sys_id[5], sys_id[6], sys_id[7]);
memcpy(pkt_basic_id.uas_id, buffer, sizeof(pkt_basic_id.uas_id));
} else
#endif
if (id_len > 0) {
// prepare basic id pkt
uint8_t val = gcs().sysid_this_mav();
pkt_basic_id.target_system = val;
pkt_basic_id.target_component = MAV_COMP_ID_ODID_TXRX_1;
pkt_basic_id.id_type = atoi(id_type);
pkt_basic_id.ua_type = atoi(ua_type);
char buffer[21];
snprintf(buffer, sizeof(buffer), "%s", id_str);
memcpy(pkt_basic_id.uas_id, buffer, sizeof(pkt_basic_id.uas_id));
}
}
void AP_OpenDroneID::get_persistent_params(ExpandingString &str) const
{
if ((pkt_basic_id.id_type == MAV_ODID_ID_TYPE_SERIAL_NUMBER)
&& (_options & LockUASIDOnFirstBasicIDRx)
&& id_len == 0) {
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "OpenDroneID: ID is locked as %s", pkt_basic_id.uas_id);
str.printf("DID_UAS_ID=%s\nDID_UAS_ID_TYPE=%u\nDID_UA_TYPE=%u\n", pkt_basic_id.uas_id, pkt_basic_id.id_type, pkt_basic_id.ua_type);
}
}
// Perform the pre-arm checks and prevent arming if they are not satisifed
@ -147,6 +229,16 @@ void AP_OpenDroneID::update()
return;
}
if ((pkt_basic_id.id_type == MAV_ODID_ID_TYPE_SERIAL_NUMBER)
&& (_options & LockUASIDOnFirstBasicIDRx)
&& id_len == 0) {
hal.util->flash_bootloader();
// reset the basic id on next set_basic_id call
pkt_basic_id.id_type = MAV_ODID_ID_TYPE_NONE;
}
set_basic_id();
const bool armed = hal.util->get_soft_armed();
if (armed && !_was_armed) {
// use arm location as takeoff location
@ -655,6 +747,9 @@ float AP_OpenDroneID::create_location_timestamp(float timestamp) const
// handle a message from the GCS
void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
{
if (!_initialised) {
return;
}
WITH_SEMAPHORE(_sem);
switch (msg.msgid) {
@ -674,7 +769,13 @@ void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t
mavlink_msg_open_drone_id_self_id_decode(&msg, &pkt_self_id);
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID:
mavlink_msg_open_drone_id_basic_id_decode(&msg, &pkt_basic_id);
if (id_len == 0
#if defined(OPENDRONEID_UA_MFR_CODE)
&& !(_options & UseChipIDAsBasicID)
#endif
) {
mavlink_msg_open_drone_id_basic_id_decode(&msg, &pkt_basic_id);
}
break;
case MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM:
mavlink_msg_open_drone_id_system_decode(&msg, &pkt_system);

View File

@ -92,6 +92,12 @@ public:
void set_arm_status(mavlink_open_drone_id_arm_status_t &status);
void set_basic_id();
void get_persistent_params(ExpandingString &str) const;
void load_UAS_ID_from_persistent_memory();
// get singleton instance
static AP_OpenDroneID *get_singleton()
{
@ -99,7 +105,7 @@ public:
}
private:
static AP_OpenDroneID *_singleton;
bool _initialised;
// parameters
AP_Int8 _enable;
AP_Float _baro_accuracy; // Vertical accuracy of the barometer when installed
@ -107,9 +113,16 @@ private:
AP_Int8 _mav_port;
AP_Int8 _can_driver;
char ua_type[3];
char id_type[3];
size_t id_len;
char id_str[21];
enum Options : int16_t {
EnforceArming = (1U << 0U),
AllowNonGPSPosition = (1U << 1U),
LockUASIDOnFirstBasicIDRx = (1U << 2U),
UseChipIDAsBasicID = (1U << 3U),
};
// check if an option is set