void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg) { if (!needToQuery(msg.getSrcNodeID())) { return; } NodeData* data = node_map_.access(msg.getSrcNodeID()); if (data == NULL) { trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get()); data = node_map_.insert(msg.getSrcNodeID(), NodeData()); if (data == NULL) { getNode().registerInternalFailure("NodeDiscoverer OOM"); return; } } UAVCAN_ASSERT(data != NULL); if (msg.uptime_sec < data->last_seen_uptime) { trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get()); data->num_get_node_info_attempts = 0; } data->last_seen_uptime = msg.uptime_sec; if (!isRunning()) { startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } }
void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg) { const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; UAVCAN_ASSERT(!since_prev_msg.isNegative()); const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); const bool switch_master = msg.getSrcNodeID() < master_nid_; // TODO: Make configurable const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS; if (switch_master || pub_timeout || needs_init) { UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", int(needs_init), int(switch_master), int(pub_timeout)); updateFromMsg(msg); } else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) { if (state_ == Adjust) { const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS; if (msg_invalid || wrong_tid || wrong_timing) { UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", int(msg_invalid), int(wrong_tid), int(wrong_timing)); state_ = Update; } } if (state_ == Adjust) { adjustFromMsg(msg); } else { updateFromMsg(msg); } } else { UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); } }
ReceivedDataStructureCopy(const ReceivedDataStructure& s) : ts_monotonic(s.getMonotonicTimestamp()), ts_utc(s.getUtcTimestamp()), transfer_type(s.getTransferType()), transfer_id(s.getTransferID()), src_node_id(s.getSrcNodeID()), iface_index(s.getIfaceIndex()), msg(s) { }
void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg) { Entry new_entry; new_entry.time_since_last_update_ms100 = 0; new_entry.status.health = msg.health & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1); new_entry.status.mode = msg.mode & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1); new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1); changeNodeStatus(msg.getSrcNodeID(), new_entry); handleNodeStatusMessage(msg); }
virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg) { Entry& entry = getEntry(msg.getSrcNodeID()); if (msg.uptime_sec < entry.uptime_sec) { entry.request_needed = true; entry.num_attempts_made = 0; startTimerIfNotRunning(); } entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>( &INodeInfoListener::handleNodeStatusMessage, msg)); }
virtual void handleReceivedDataStruct(ReceivedDataStructure<RequestType>& request) { UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); ServiceResponseDataStructure<ResponseType> response; if (coerceOrFallback<bool>(callback_, true)) { UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default callback_(request, response); } else { handleFatalError("Srv serv clbk"); } if (response.isResponseEnabled()) { publisher_.setPriority(request.getPriority()); // Responding at the same priority. const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), request.getTransferID()); if (res < 0) { UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res); publisher_.getNode().getDispatcher().getTransferPerfCounter().addError(); response_failure_count_++; } } else { UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application"); } }
void handleAllocation(const ReceivedDataStructure<Allocation>& msg) { trace(TraceAllocationActivity, msg.getSrcNodeID().get()); if (!msg.isAnonymousTransfer()) { return; // This is a response from another allocator, ignore } /* * Reset the expected stage on timeout */ if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_)) { UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); current_unique_id_.clear(); trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec()); } /* * Checking if request stage matches the expected stage */ const uint8_t request_stage = detectRequestStage(msg); if (request_stage == InvalidStage) { trace(TraceAllocationBadRequest, msg.unique_id.size()); return; // Malformed request - ignore without resetting } const uint8_t expected_stage = getExpectedStage(); if (expected_stage == InvalidStage) { UAVCAN_ASSERT(0); return; } if (request_stage != expected_stage) { trace(TraceAllocationUnexpectedStage, request_stage); return; // Ignore - stage mismatch } const uint8_t max_expected_bytes = static_cast<uint8_t>(current_unique_id_.capacity() - current_unique_id_.size()); UAVCAN_ASSERT(max_expected_bytes > 0); if (msg.unique_id.size() > max_expected_bytes) { trace(TraceAllocationBadRequest, msg.unique_id.size()); return; // Malformed request } /* * Updating the local state */ for (uint8_t i = 0; i < msg.unique_id.size(); i++) { current_unique_id_.push_back(msg.unique_id[i]); } trace(TraceAllocationRequestAccepted, current_unique_id_.size()); /* * Proceeding with allocation if possible * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader. */ if (current_unique_id_.size() == current_unique_id_.capacity()) { UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d", int(msg.node_id)); UniqueID unique_id; copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); current_unique_id_.clear(); { uint64_t event_agrument = 0; for (uint8_t i = 0; i < 8; i++) { event_agrument |= static_cast<uint64_t>(unique_id[i]) << (56U - i * 8U); } trace(TraceAllocationExchangeComplete, static_cast<int64_t>(event_agrument)); } handler_.handleAllocationRequest(unique_id, msg.node_id); } else { if (handler_.canPublishFollowupAllocationResponse()) { publishFollowupAllocationResponse(); } else { trace(TraceAllocationFollowupDenied, 0); current_unique_id_.clear(); } } /* * It is super important to update timestamp only if the request has been processed successfully. */ last_message_timestamp_ = msg.getMonotonicTimestamp(); }
void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg) { /* * TODO This method can blow the stack easily */ UAVCAN_ASSERT(preferred_node_id_.isValid()); if (isAllocationComplete()) { UAVCAN_ASSERT(0); terminate(); return; } startPeriodic(getPeriod()); // Restarting the timer - Rule C UAVCAN_TRACE("DynamicNodeIDClient", "Request timer reset because of Allocation message from %d", static_cast<int>(msg.getSrcNodeID().get())); // Rule D if (!msg.isAnonymousTransfer() && msg.unique_id.size() > 0 && msg.unique_id.size() < msg.unique_id.capacity() && equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) { // Filling the response message const uint8_t size_of_unique_id_in_response = min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, static_cast<uint8_t>(msg.unique_id.capacity() - msg.unique_id.size())); protocol::dynamic_node_id::Allocation second_stage; second_stage.node_id = preferred_node_id_.get(); second_stage.first_part_of_unique_id = false; second_stage.unique_id.resize(size_of_unique_id_in_response); copy(unique_id_ + msg.unique_id.size(), unique_id_ + msg.unique_id.size() + size_of_unique_id_in_response, second_stage.unique_id.begin()); UAVCAN_ASSERT(equal(second_stage.unique_id.begin(), second_stage.unique_id.end(), unique_id_ + msg.unique_id.size())); // Broadcasting the response UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d", static_cast<int>(preferred_node_id_.get()), static_cast<int>(second_stage.unique_id.size())); const int res = dnida_pub_.broadcast(second_stage); if (res < 0) { dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); } } // Rule E if (!msg.isAnonymousTransfer() && msg.unique_id.size() == msg.unique_id.capacity() && equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) && msg.node_id > 0) { allocated_node_id_ = msg.node_id; allocator_node_id_ = msg.getSrcNodeID(); UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d", static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get())); terminate(); UAVCAN_ASSERT(isAllocationComplete()); } }