mirror of https://github.com/ArduPilot/ardupilot
AP_OpenDroneID: add support for persistent storage of UAS ID
This commit is contained in:
bugobliterator
Peter Barker
parent
ff3f07d46f
commit
53f7315bb5
|
|
@ -39,6 +39,8 @@
|
|||
#include <AP_Parachute/AP_Parachute.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <AP_DroneCAN/AP_DroneCAN.h>
|
||||
#include <stdio.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
|
|
@ -65,7 +67,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
|
||||
|
|
@ -78,6 +80,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()
|
||||
{
|
||||
|
|
@ -96,7 +105,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
|
||||
|
|
@ -148,6 +230,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
|
||||
|
|
@ -670,6 +762,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) {
|
||||
|
|
@ -689,7 +784,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:
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue