diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index c2719838b1..db856ed560 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include "AP_UAVCAN_Clock.h" extern const AP_HAL::HAL& hal; @@ -427,6 +428,12 @@ void AP_UAVCAN_DNA_Server::verify_nodes(AP_UAVCAN *ap_uavcan) return; } + uint8_t log_count = AP::logger().get_log_start_count(); + if (log_count != last_logging_count) { + last_logging_count = log_count; + logged.clearall(); + } + //Check if we got acknowledgement from previous request //except for requests using our own node_id for (uint8_t i = 0; i < HAL_MAX_CAN_PROTOCOL_DRIVERS; i++) { @@ -502,12 +509,31 @@ Handle responses from GetNodeInfo Request. We verify the node info against our records. Marks Verification mask if already recorded, Or register if the node id is available and not recorded for the received Unique ID */ -void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[]) +void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit) { if (node_id > MAX_NODE_ID) { return; } WITH_SEMAPHORE(sem); + + /* + if we haven't logged this node then log it now + */ + if (!logged.get(node_id) && AP::logger().logging_started()) { + logged.set(node_id); + uint64_t uid[2]; + memcpy(uid, unique_id, sizeof(uid)); + AP::logger().Write("CAND", "TimeUS,NodeId,UID1,UID2,Name,Major,Minor,Version", + "s#------", "F-------", "QBQQZBBI", + AP_HAL::micros64(), + node_id, + uid[0], uid[1], + name, + major, + minor, + vcs_commit); + } + if (isNodeIDOccupied(node_id)) { //if node_id already registered, just verify if Unique ID matches as well if (node_id == getNodeIDForUniqueID(unique_id, 16)) { @@ -544,16 +570,23 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], void trampoline_handleNodeInfo(const uavcan::ServiceCallResult& resp) { uint8_t node_id, unique_id[16] = {0}; - char name[15] = {0}; + char name[50] = {0}; node_id = resp.getResponse().getSrcNodeID().get(); //copy the unique id from message to uint8_t array - uavcan::copy(resp.getResponse().hardware_version.unique_id.begin(), - resp.getResponse().hardware_version.unique_id.end(), + auto &r = resp.getResponse(); + uavcan::copy(r.hardware_version.unique_id.begin(), + r.hardware_version.unique_id.end(), unique_id); - strncpy_noterm(name, resp.getResponse().name.c_str(), sizeof(name)-1); - AP::uavcan_dna_server().handleNodeInfo(node_id, unique_id, name); + strncpy_noterm(name, r.name.c_str(), sizeof(name)-1); + + auto &dna_server = AP::uavcan_dna_server(); + + dna_server.handleNodeInfo(node_id, unique_id, name, + r.software_version.major, + r.software_version.minor, + r.software_version.vcs_commit); } /* Handle the allocation message from the devices supporting diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h index 78b8ca2fb5..91eec40837 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h @@ -37,6 +37,9 @@ class AP_UAVCAN_DNA_Server Bitmask<128> occupation_mask; Bitmask<128> verified_mask; Bitmask<128> node_seen_mask; + Bitmask<128> logged; + + uint8_t last_logging_count; //Error State enum ServerState server_state; @@ -115,7 +118,7 @@ public: //Callbacks void handleAllocation(uint8_t driver_index, uint8_t node_id, const AllocationCb &cb); void handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb); - void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[]); + void handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char name[], uint8_t major, uint8_t minor, uint32_t vcs_commit); //Run through the list of seen node ids for verification void verify_nodes(AP_UAVCAN *ap_uavcan);