mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-30 08:04:08 +08:00
77 lines
2.3 KiB
C++
77 lines
2.3 KiB
C++
/*
|
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <bitset>
|
|
#include <uavcan/util/method_binder.hpp>
|
|
#include <uavcan/node/subscriber.hpp>
|
|
#include <uavcan/node/service_client.hpp>
|
|
#include <uavcan/protocol/ComputeAggregateTypeSignature.hpp>
|
|
#include <uavcan/protocol/NodeStatus.hpp>
|
|
|
|
namespace uavcan
|
|
{
|
|
|
|
struct UAVCAN_EXPORT NodeInitializationResult
|
|
{
|
|
NodeID conflicting_node;
|
|
bool isOk() const { return !conflicting_node.isValid(); }
|
|
};
|
|
|
|
/**
|
|
* This class does not issue GlobalDiscoveryRequest, assuming that it was done already by the caller.
|
|
* Instantiated object can execute() only once. Objects of this class are intended for stack allocation.
|
|
*/
|
|
class UAVCAN_EXPORT NodeInitializer : Noncopyable
|
|
{
|
|
typedef std::bitset<NodeID::Max + 1> NodeIDMask;
|
|
typedef MethodBinder<NodeInitializer*,
|
|
void (NodeInitializer::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
|
|
NodeStatusCallback;
|
|
typedef MethodBinder<NodeInitializer*,
|
|
void (NodeInitializer::*)(ServiceCallResult<protocol::ComputeAggregateTypeSignature>&)>
|
|
CATSResponseCallback;
|
|
|
|
Subscriber<protocol::NodeStatus, NodeStatusCallback> ns_sub_;
|
|
ServiceClient<protocol::ComputeAggregateTypeSignature, CATSResponseCallback> cats_cln_;
|
|
NodeIDMask nid_mask_present_;
|
|
NodeIDMask nid_mask_checked_;
|
|
NodeInitializationResult result_;
|
|
DataTypeKind checking_dtkind_;
|
|
bool last_cats_request_ok_;
|
|
|
|
INode& getNode() { return ns_sub_.getNode(); }
|
|
const INode& getNode() const { return ns_sub_.getNode(); }
|
|
|
|
MonotonicDuration getNetworkDiscoveryDelay() const;
|
|
|
|
NodeID findNextUncheckedNode();
|
|
|
|
int waitForCATSResponse();
|
|
|
|
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg);
|
|
void handleCATSResponse(ServiceCallResult<protocol::ComputeAggregateTypeSignature>& resp);
|
|
|
|
int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind);
|
|
int checkOneNode(NodeID nid);
|
|
int checkNodes();
|
|
|
|
public:
|
|
NodeInitializer(INode& node)
|
|
: ns_sub_(node)
|
|
, cats_cln_(node)
|
|
, checking_dtkind_(DataTypeKindService)
|
|
, last_cats_request_ok_(false)
|
|
{ }
|
|
|
|
int execute();
|
|
|
|
const NodeInitializationResult& getResult() const { return result_; }
|
|
|
|
static int publishGlobalDiscoveryRequest(INode& node);
|
|
};
|
|
|
|
}
|