Line data    Source code

       1             : #pragma once
       2             : #include <AP_HAL/AP_HAL_Boards.h>
       3             : #include <AP_HAL/Semaphores.h>
       4             : 
       5             : #if HAL_ENABLE_DRONECAN_DRIVERS
       6             : #include <AP_Common/Bitmask.h>
       7             : #include <StorageManager/StorageManager.h>
       8             : #include <AP_CANManager/AP_CANManager.h>
       9             : #include <canard/publisher.h>
      10             : #include <canard/subscriber.h>
      11             : #include <canard/service_client.h>
      12             : #include "AP_Canard_iface.h"
      13             : #include <dronecan_msgs.h>
      14             : 
      15             : class AP_DroneCAN;
      16             : //Forward declaring classes
      17             : class AP_DroneCAN_DNA_Server
      18             : {
      19             :     StorageAccess storage;
      20             : 
      21             :     struct NodeRecord {
      22             :         uint8_t uid_hash[6];
      23             :         uint8_t crc;
      24             :     };
      25             : 
      26             :     /*
      27             :      * For each node ID (1 through MAX_NODE_ID), the database can have one
      28             :      * registration for it. Each registration consists of a NodeRecord which
      29             :      * contains the (hash of the) unique ID reported by that node ID. Other
      30             :      * info could be added to the registration in the future.
      31             :      *
      32             :      * Physically, the database is stored as a header and format version,
      33             :      * followed by an array of NodeRecords indexed by node ID. If a particular
      34             :      * NodeRecord has an all-zero unique ID hash or an invalid CRC, then that
      35             :      * node ID isn't considerd to have a registration.
      36             :      *
      37             :      * The database has public methods which handle the server behavior for the
      38             :      * relevant message. The methods can be used by multiple servers in
      39             :      * different threads, so each holds a lock for its duration.
      40             :      */
      41             :     class Database {
      42             :     public:
      43        2193 :         Database() {};
      44             : 
      45             :         // initialize database (storage accessor is always replaced with the one supplied)
      46             :         void init(StorageAccess *storage_);
      47             : 
      48             :         // remove all registrations from the database
      49             :         void reset();
      50             : 
      51             :         // return true if the given node ID is registered
      52         109 :         bool is_registered(uint8_t node_id) {
      53         109 :             return node_registered.get(node_id);
      54             :         }
      55             : 
      56             :         // handle initializing the server with its own node ID and unique ID
      57             :         void init_server(uint8_t own_node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len);
      58             : 
      59             :         // handle processing the node info message. returns true if from a duplicate node
      60             :         bool handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]);
      61             : 
      62             :         // handle the allocation message. returns the allocated node ID, or 0 if allocation failed
      63             :         uint8_t handle_allocation(const uint8_t unique_id[]);
      64             : 
      65             :     private:
      66             :         // retrieve node ID that matches the given unique ID. returns 0 if not found
      67             :         uint8_t find_node_id(const uint8_t unique_id[], uint8_t size);
      68             : 
      69             :         // fill the given record with the hash of the given unique ID
      70             :         void compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const;
      71             : 
      72             :         // register a given unique ID to a given node ID, deleting any existing registration for the unique ID
      73             :         void register_uid(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
      74             : 
      75             :         // create the registration for the given node ID and set its record's unique ID
      76             :         void create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
      77             : 
      78             :         // delete the given node ID's registration
      79             :         void delete_registration(uint8_t node_id);
      80             : 
      81             :         // return true if the given node ID has a registration
      82             :         bool check_registration(uint8_t node_id);
      83             : 
      84             :         // read the given node ID's registration's record
      85             :         void read_record(NodeRecord &record, uint8_t node_id);
      86             : 
      87             :         // write the given node ID's registration's record
      88             :         void write_record(const NodeRecord &record, uint8_t node_id);
      89             : 
      90             :         // bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID)
      91             :         Bitmask<128> node_registered; // have a registration for this node ID
      92             : 
      93             :         StorageAccess *storage;
      94             :         HAL_Semaphore sem;
      95             :     };
      96             : 
      97             :     static Database db;
      98             : 
      99             :     enum ServerState {
     100             :         NODE_STATUS_UNHEALTHY = -5,
     101             :         DUPLICATE_NODES = -2,
     102             :         HEALTHY = 0
     103             :     };
     104             : 
     105             :     uint32_t last_verification_request;
     106             :     uint8_t curr_verifying_node;
     107             :     uint8_t self_node_id;
     108             :     bool nodeInfo_resp_rcvd;
     109             : 
     110             :     // bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID)
     111             :     Bitmask<128> node_verified; // node seen and unique ID matches stored
     112             :     Bitmask<128> node_seen; // received NodeStatus
     113             :     Bitmask<128> node_logged; // written to log fle
     114             :     Bitmask<128> node_healthy; // reports healthy
     115             : 
     116             :     uint8_t last_logging_count;
     117             : 
     118             :     //Error State
     119             :     enum ServerState server_state;
     120             :     uint8_t fault_node_id;
     121             :     char fault_node_name[15];
     122             : 
     123             : 
     124             :     // dynamic node ID allocation state variables
     125             :     uint8_t rcvd_unique_id[16];
     126             :     uint8_t rcvd_unique_id_offset;
     127             :     uint32_t last_alloc_msg_ms;
     128             : 
     129             :     AP_DroneCAN &_ap_dronecan;
     130             :     CanardInterface &_canard_iface;
     131             : 
     132             :     Canard::Publisher<uavcan_protocol_dynamic_node_id_Allocation> allocation_pub{_canard_iface};
     133             : 
     134             :     Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_dynamic_node_id_Allocation> allocation_cb{this, &AP_DroneCAN_DNA_Server::handle_allocation};
     135             :     Canard::Subscriber<uavcan_protocol_dynamic_node_id_Allocation> allocation_sub;
     136             : 
     137             :     Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_NodeStatus> node_status_cb{this, &AP_DroneCAN_DNA_Server::handleNodeStatus};
     138             :     Canard::Subscriber<uavcan_protocol_NodeStatus> node_status_sub;
     139             : 
     140             :     Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_GetNodeInfoResponse> node_info_cb{this, &AP_DroneCAN_DNA_Server::handleNodeInfo};
     141             :     Canard::Client<uavcan_protocol_GetNodeInfoResponse> node_info_client;
     142             : 
     143             : #if HAL_LOGGING_ENABLED
     144             :     // Linked list of node status timestamps used for logging
     145             :     struct node_status_log_data {
     146           2 :         node_status_log_data(const uint8_t _id):id(_id) {};
     147             :         node_status_log_data *next;
     148             :         uint32_t last_uptime_sec;
     149             :         uint32_t last_log_ms;
     150             :         const uint8_t id;
     151             :     };
     152             :     node_status_log_data *node_status_list;
     153             :     node_status_log_data *find_node_status_item(const uint8_t source_node_id);
     154             :     void update_node_status(const uint8_t source_node_id, const uavcan_protocol_NodeStatus& msg);
     155             : #endif // #if HAL_LOGGING_ENABLED
     156             : 
     157             : public:
     158             :     AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface, uint8_t driver_index);
     159             : 
     160             : 
     161             :     // Do not allow copies
     162             :     CLASS_NO_COPY(AP_DroneCAN_DNA_Server);
     163             : 
     164             :     //Initialises publisher and Server Record for specified uavcan driver
     165             :     bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id);
     166             : 
     167             :     //report the server state, along with failure message if any
     168             :     bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
     169             : 
     170             :     // canard message handler callbacks
     171             :     void handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
     172             :     void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg);
     173             :     void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp);
     174             : 
     175             :     //Run through the list of seen node ids for verification
     176             :     void verify_nodes();
     177             : };
     178             : 
     179             : #endif