ardupilot/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h

Ignoring revisions in .git-blame-ignore-revs. Click here to bypass and see the normal blame view.

172 lines
6.5 KiB
C
Raw Normal View History

#pragma once
2022-02-28 21:19:20 -04:00
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_HAL/Semaphores.h>
#if HAL_ENABLE_DRONECAN_DRIVERS
#include <AP_Common/Bitmask.h>
#include <StorageManager/StorageManager.h>
#include <AP_CANManager/AP_CANManager.h>
#include <canard/publisher.h>
#include <canard/subscriber.h>
#include <canard/service_client.h>
#include "AP_Canard_iface.h"
#include <dronecan_msgs.h>
class AP_DroneCAN;
//Forward declaring classes
class AP_DroneCAN_DNA_Server
{
StorageAccess storage;
struct NodeRecord {
uint8_t uid_hash[6];
uint8_t crc;
};
/*
* For each node ID (1 through MAX_NODE_ID), the database can have one
* registration for it. Each registration consists of a NodeRecord which
* contains the (hash of the) unique ID reported by that node ID. Other
* info could be added to the registration in the future.
*
* Physically, the database is stored as a header and format version,
* followed by an array of NodeRecords indexed by node ID. If a particular
* NodeRecord has an all-zero unique ID hash or an invalid CRC, then that
* node ID isn't considerd to have a registration.
*
* The database has public methods which handle the server behavior for the
* relevant message. The methods can be used by multiple servers in
* different threads, so each holds a lock for its duration.
*/
class Database {
public:
Database() {};
// initialize database (storage accessor is always replaced with the one supplied)
void init(StorageAccess *storage_);
// remove all registrations from the database
void reset();
// return true if the given node ID is registered
bool is_registered(uint8_t node_id) {
return node_registered.get(node_id);
}
// handle initializing the server with its own node ID and unique ID
void init_server(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len);
// handle processing the node info message. returns true if from a duplicate node
bool handle_node_info(uint8_t source_node_id, const uint8_t unique_id[]);
// handle the allocation message. returns the allocated node ID, or 0 if allocation failed
uint8_t handle_allocation(uint8_t node_id, const uint8_t unique_id[]);
private:
// search for a free node ID, starting at the preferred ID (which can be 0 if
// none are preferred). returns 0 if none found. based on pseudocode in
// uavcan/protocol/dynamic_node_id/1.Allocation.uavcan
uint8_t find_free_node_id(uint8_t preferred);
// retrieve node ID that matches the given unique ID. returns 0 if not found
uint8_t find_node_id(const uint8_t unique_id[], uint8_t size);
// fill the given record with the hash of the given unique ID
void compute_uid_hash(NodeRecord &record, const uint8_t unique_id[], uint8_t size) const;
// create the registration for the given node ID and set its record's unique ID
void create_registration(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
// delete the given node ID's registration
void delete_registration(uint8_t node_id);
// return true if the given node ID has a registration
bool check_registration(uint8_t node_id);
// read the given node ID's registration's record
void read_record(NodeRecord &record, uint8_t node_id);
// write the given node ID's registration's record
void write_record(const NodeRecord &record, uint8_t node_id);
// bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID)
Bitmask<128> node_registered; // have a registration for this node ID
StorageAccess *storage;
HAL_Semaphore sem;
};
static Database db;
enum ServerState {
2022-04-29 08:47:51 -03:00
NODE_STATUS_UNHEALTHY = -5,
DUPLICATE_NODES = -2,
HEALTHY = 0
};
uint32_t last_verification_request;
uint8_t curr_verifying_node;
uint8_t self_node_id;
bool nodeInfo_resp_rcvd;
// bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID)
Bitmask<128> node_verified; // node seen and unique ID matches stored
Bitmask<128> node_seen; // received NodeStatus
Bitmask<128> node_logged; // written to log fle
Bitmask<128> node_healthy; // reports healthy
uint8_t last_logging_count;
//Error State
enum ServerState server_state;
uint8_t fault_node_id;
char fault_node_name[15];
// dynamic node ID allocation state variables
uint8_t rcvd_unique_id[16];
uint8_t rcvd_unique_id_offset;
uint32_t last_alloc_msg_ms;
AP_DroneCAN &_ap_dronecan;
CanardInterface &_canard_iface;
Canard::Publisher<uavcan_protocol_dynamic_node_id_Allocation> allocation_pub{_canard_iface};
Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_dynamic_node_id_Allocation> allocation_cb{this, &AP_DroneCAN_DNA_Server::handle_allocation};
Canard::Subscriber<uavcan_protocol_dynamic_node_id_Allocation> allocation_sub;
Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_NodeStatus> node_status_cb{this, &AP_DroneCAN_DNA_Server::handleNodeStatus};
Canard::Subscriber<uavcan_protocol_NodeStatus> node_status_sub;
Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_GetNodeInfoResponse> node_info_cb{this, &AP_DroneCAN_DNA_Server::handleNodeInfo};
Canard::Client<uavcan_protocol_GetNodeInfoResponse> node_info_client;
public:
AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan, CanardInterface &canard_iface, uint8_t driver_index);
// Do not allow copies
CLASS_NO_COPY(AP_DroneCAN_DNA_Server);
//Initialises publisher and Server Record for specified uavcan driver
bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id);
/* Subscribe to the messages to be handled for maintaining and allocating
Node ID list */
static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
//report the server state, along with failure message if any
bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
// canard message handler callbacks
void handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg);
void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp);
//Run through the list of seen node ids for verification
void verify_nodes();
};
#endif