db18d37ed2
Avoids confusing the user and removes weirdness with multiple servers sharing the same storage. Does leak the registration for the old ID but in the unlikely event the table fills up the user can simply reset the database. We keep the check for an existing registration to avoid dirtying the storage every boot unnecessarily. We also factor out the deletion of an existing registration (which is very unlikely but technically possible) to save some flash.
175 lines
6.7 KiB
C++
175 lines
6.7 KiB
C++
#pragma once
|
|
#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 own_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;
|
|
|
|
// register a given unique ID to a given node ID, deleting any existing registration for the unique ID
|
|
void register_uid(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
|
|
|
|
// 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 {
|
|
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
|