diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp index 94524936ce..77740c7c63 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp +++ b/libraries/AP_OpenDroneID/AP_OpenDroneID.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include 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: - 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); diff --git a/libraries/AP_OpenDroneID/AP_OpenDroneID.h b/libraries/AP_OpenDroneID/AP_OpenDroneID.h index 0ebd047685..585865ef2f 100644 --- a/libraries/AP_OpenDroneID/AP_OpenDroneID.h +++ b/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