AP_OpenDroneID: add support for persistent storage of UAS ID

libraries/AP_OpenDroneID/AP_OpenDroneID.cpp

@@ -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);

libraries/AP_OpenDroneID/AP_OpenDroneID.h

@@ -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