bool TransferReceiver::validate(const RxFrame& frame) const
{
if (iface_index_ != frame.getIfaceIndex())
{
return false;
}
if (frame.isFirst() && !frame.isLast() && (frame.getPayloadLen() < TransferCRC::NumBytes))
{
UAVCAN_TRACE("TransferReceiver", "CRC expected, %s", frame.toString().c_str());
registerError();
return false;
}
if ((frame.getIndex() == Frame::MaxIndex) && !frame.isLast())
{
UAVCAN_TRACE("TransferReceiver", "Unterminated transfer, %s", frame.toString().c_str());
registerError();
return false;
}
if (frame.getIndex() != next_frame_index_)
{
UAVCAN_TRACE("TransferReceiver", "Unexpected frame index (not %i), %s",
int(next_frame_index_), frame.toString().c_str());
registerError();
return false;
}
if (getTidRelation(frame) != TidSame)
{
UAVCAN_TRACE("TransferReceiver", "Unexpected TID (current %i), %s", tid_.get(), frame.toString().c_str());
registerError();
return false;
}
return true;
}
NodeID pickNextNodeID() const
{
// We can't do index search because indices are unstable in Map<>
// First try - from the current node up
NextNodeIDSearchPredicate s1(*this);
(void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s1);
if (s1.output != NextNodeIDSearchPredicate::DefaultOutput)
{
last_queried_node_id_ = s1.output;
}
else if (last_queried_node_id_ != 0)
{
// Nothing was found, resetting the selector and trying again
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_));
last_queried_node_id_ = 0;
NextNodeIDSearchPredicate s2(*this);
(void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s2);
if (s2.output != NextNodeIDSearchPredicate::DefaultOutput)
{
last_queried_node_id_ = s2.output;
}
}
else
{
; // Hopeless
}
UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u",
int(last_queried_node_id_), pending_nodes_.getSize(),
begin_fw_update_client_.getNumPendingCalls());
return last_queried_node_id_;
}
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 handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
{
if (result.isSuccessful())
{
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
int(result.getCallID().server_node_id.get()));
finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
}
else
{
trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
NodeData* const data = node_map_.access(result.getCallID().server_node_id);
if (data == NULL)
{
return; // Probably it is a known node now
}
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
"GetNodeInfo request to %d has timed out, %d attempts",
int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
data->num_get_node_info_attempts++;
if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
{
finalizeNodeDiscovery(NULL, result.getCallID().server_node_id);
// Warning: data pointer is invalidated now
}
}
}
/*
* TransferListenerBase
*/
bool TransferListenerBase::checkPayloadCrc(const uint16_t compare_with, const ITransferBuffer& tbb) const
{
TransferCRC crc = crc_base_;
unsigned offset = 0;
while (true)
{
uint8_t buf[16];
const int res = tbb.read(offset, buf, sizeof(buf));
if (res < 0)
{
UAVCAN_TRACE("TransferListenerBase", "Failed to check CRC: Buffer read failure %i", res);
return false;
}
if (res == 0)
{
break;
}
offset += unsigned(res);
crc.add(buf, unsigned(res));
}
if (crc.get() != compare_with)
{
UAVCAN_TRACE("TransferListenerBase", "CRC mismatch, expected=0x%04x, got=0x%04x",
int(compare_with), int(crc.get()));
return false;
}
return true;
}
TransferReceiver::ResultCode TransferReceiver::receive(const RxFrame& frame, TransferBufferAccessor& tba)
{
// Transfer timestamps are derived from the first frame
if (frame.isFirst())
{
this_transfer_ts_ = frame.getMonotonicTimestamp();
first_frame_ts_ = frame.getUtcTimestamp();
}
if (frame.isFirst() && frame.isLast())
{
tba.remove();
updateTransferTimings();
prepareForNextTransfer();
this_transfer_crc_ = 0; // SFT has no CRC
return ResultSingleFrame;
}
// Payload write
ITransferBuffer* buf = tba.access();
if (buf == NULL)
{
buf = tba.create();
}
if (buf == NULL)
{
UAVCAN_TRACE("TransferReceiver", "Failed to access the buffer, %s", frame.toString().c_str());
prepareForNextTransfer();
registerError();
return ResultNotComplete;
}
if (!writePayload(frame, *buf))
{
UAVCAN_TRACE("TransferReceiver", "Payload write failed, %s", frame.toString().c_str());
tba.remove();
prepareForNextTransfer();
registerError();
return ResultNotComplete;
}
next_frame_index_++;
if (frame.isLast())
{
updateTransferTimings();
prepareForNextTransfer();
return ResultComplete;
}
return ResultNotComplete;
}
void DataTypeInfoProvider::handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request,
protocol::GetDataTypeInfo::Response& response)
{
const DataTypeKind kind = DataTypeKind(request.kind.value);
if (!isValidDataTypeKind(kind))
{
UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i", kind);
return;
}
const DataTypeDescriptor* const desc = GlobalDataTypeRegistry::instance().find(kind, request.id);
if (!desc)
{
UAVCAN_TRACE("DataTypeInfoProvider", "Cannot process GetDataTypeInfo for nonexistent type dtid=%i dtk=%i",
int(request.id), int(request.kind.value));
return;
}
UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str());
response.signature = desc->getSignature().get();
response.name = desc->getFullName();
response.mask = protocol::GetDataTypeInfo::Response::MASK_KNOWN;
const Dispatcher& dispatcher = getNode().getDispatcher();
if (request.kind.value == protocol::DataTypeKind::SERVICE)
{
if (dispatcher.hasServer(request.id))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_SERVING;
}
}
else if (request.kind.value == protocol::DataTypeKind::MESSAGE)
{
if (dispatcher.hasSubscriber(request.id))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_SUBSCRIBED;
}
if (dispatcher.hasPublisher(request.id))
{
response.mask |= protocol::GetDataTypeInfo::Response::MASK_PUBLISHING;
}
}
else
{
assert(0); // That means that GDTR somehow found a type of an unknown kind. The horror.
}
}
void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&,
protocol::GetNodeInfo::Response& rsp)
{
UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request");
UAVCAN_ASSERT(isNodeInfoInitialized());
rsp = node_info_;
}
ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key)
{
if (key.isEmpty())
{
UAVCAN_ASSERT(0);
return UAVCAN_NULLPTR;
}
remove(key);
TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_);
if (tbme == UAVCAN_NULLPTR)
{
return UAVCAN_NULLPTR; // Epic fail.
}
buffers_.insert(tbme);
UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str());
if (tbme != UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(tbme->isEmpty());
tbme->reset(key);
}
return tbme;
}
virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
{
const bool was_offline = !event.old_status.known ||
(event.old_status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
const bool offline_now = !event.status.known ||
(event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
if (was_offline || offline_now)
{
Entry& entry = getEntry(event.node_id);
entry.request_needed = !offline_now;
entry.num_attempts_made = 0;
UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
int(event.node_id.get()), int(entry.request_needed));
if (entry.request_needed)
{
startTimerIfNotRunning();
}
}
listeners_.forEach(
GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
}
NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
{
out_at_least_one_request_needed = false;
for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
{
last_picked_node_++;
if (last_picked_node_ > NodeID::Max)
{
last_picked_node_ = 1;
}
UAVCAN_ASSERT((last_picked_node_ >= 1) &&
(last_picked_node_ <= NodeID::Max));
const Entry& entry = getEntry(last_picked_node_);
if (entry.request_needed)
{
out_at_least_one_request_needed = true;
if (entry.updated_since_last_attempt &&
!get_node_info_client_.hasPendingCallToServer(last_picked_node_))
{
UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
return NodeID(last_picked_node_);
}
}
}
return NodeID(); // No node could be found
}
int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline,
MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id) const
{
/*
* TODO: TID is not needed for anonymous transfers, this part of the code can be skipped?
*/
const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id);
UAVCAN_ASSERT(!tx_deadline.isZero());
const MonotonicTime otr_deadline = tx_deadline + max(max_transfer_interval_ * 2,
OutgoingTransferRegistry::MinEntryLifetime);
TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
if (tid == UAVCAN_NULLPTR)
{
UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i",
int(data_type_id_.get()), int(transfer_type));
return -ErrMemory;
}
const TransferID this_tid = tid->get();
tid->increment();
return send(payload, payload_len, tx_deadline, blocking_deadline, transfer_type,
dst_node_id, this_tid);
}
void handleTimerEvent(const TimerEvent&)
{
if (get_node_info_client_.hasPendingCalls())
{
return;
}
const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
if (!node_id.isUnicast())
{
trace(TraceDiscoveryTimerStop, 0);
stop();
return;
}
if (!handler_.canDiscoverNewNodes())
{
return; // Timer must continue to run in order to not stuck when it unlocks
}
trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
int(node_id.get()));
const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
if (res < 0)
{
getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call");
}
}
void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&)
{
// This method implements Rule B
UAVCAN_ASSERT(preferred_node_id_.isValid());
if (isAllocationComplete())
{
UAVCAN_ASSERT(0);
terminate();
return;
}
// Filling the message
protocol::dynamic_node_id::Allocation msg;
msg.node_id = preferred_node_id_.get();
msg.first_part_of_unique_id = true;
msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST);
copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin());
UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_));
// Broadcasting
UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 1st stage: preferred ID: %d",
static_cast<int>(preferred_node_id_.get()));
const int res = dnida_pub_.broadcast(msg);
if (res < 0)
{
dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed");
}
}
virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info)
{
FirmwareFilePath firmware_file_path;
const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path);
if (update_needed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s",
int(node_id.get()), firmware_file_path.c_str());
trySetPendingNode(node_id, firmware_file_path);
}
else
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get()));
pending_nodes_.remove(node_id);
}
}
void startTimerIfNotRunning()
{
if (!TimerBase::isRunning())
{
TimerBase::startPeriodic(request_interval_);
UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str());
}
}
virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
{
if (event.status.mode == protocol::NodeStatus::MODE_OFFLINE)
{
pending_nodes_.remove(event.node_id);
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get()));
}
}
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()));
}
}
void DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest(
const protocol::ComputeAggregateTypeSignature::Request& request,
protocol::ComputeAggregateTypeSignature::Response& response)
{
const DataTypeKind kind = DataTypeKind(request.kind.value);
if (!isValidDataTypeKind(kind))
{
UAVCAN_TRACE("DataTypeInfoProvider",
"ComputeAggregateTypeSignature request with invalid DataTypeKind %i", kind);
return;
}
UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request for dtk=%i", int(request.kind.value));
response.mutually_known_ids = request.known_ids;
response.aggregate_signature =
GlobalDataTypeRegistry::instance().computeAggregateSignature(kind, response.mutually_known_ids).get();
}
void GlobalDataTypeRegistry::freeze()
{
if (!frozen_)
{
frozen_ = true;
UAVCAN_TRACE("GlobalDataTypeRegistry", "Frozen; num msgs: %u, num srvs: %u",
getNumMessageTypes(), getNumServiceTypes());
}
}
void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxFrame& frame,
TransferBufferAccessor& tba)
{
switch (receiver.addFrame(frame, tba))
{
case TransferReceiver::ResultNotComplete:
{
perf_.addErrors(receiver.yieldErrorCount());
break;
}
case TransferReceiver::ResultSingleFrame:
{
perf_.addRxTransfer();
SingleFrameIncomingTransfer it(frame);
handleIncomingTransfer(it);
break;
}
case TransferReceiver::ResultComplete:
{
perf_.addRxTransfer();
const ITransferBuffer* tbb = tba.access();
if (tbb == NULL)
{
UAVCAN_TRACE("TransferListenerBase", "Buffer access failure, last frame: %s", frame.toString().c_str());
break;
}
if (!checkPayloadCrc(receiver.getLastTransferCrc(), *tbb))
{
UAVCAN_TRACE("TransferListenerBase", "CRC error, last frame: %s", frame.toString().c_str());
break;
}
MultiFrameIncomingTransfer it(receiver.getLastTransferTimestampMonotonic(),
receiver.getLastTransferTimestampUtc(), frame, tba);
handleIncomingTransfer(it);
it.release();
break;
}
default:
{
UAVCAN_ASSERT(0);
break;
}
}
}
void handleBeginFirmwareUpdateResponse(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>& result)
{
if (!result.isSuccessful())
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry",
int(result.getCallID().server_node_id.get()));
return;
}
FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id);
if (old_path == UAVCAN_NULLPTR)
{
// The entry has been removed, assuming that it's not needed anymore
return;
}
const bool confirmed =
result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK ||
result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS;
if (confirmed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request",
int(result.getCallID().server_node_id.get()));
pending_nodes_.remove(result.getCallID().server_node_id);
checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse());
}
else
{
UAVCAN_ASSERT(old_path != UAVCAN_NULLPTR);
UAVCAN_ASSERT(TimerBase::isRunning());
// We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer
const bool update_needed =
checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path);
if (!update_needed)
{
UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry",
int(result.getCallID().server_node_id.get()));
pending_nodes_.remove(result.getCallID().server_node_id);
}
}
}
int MultiFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned len) const
{
const ITransferBuffer* const tbb = const_cast<TransferBufferAccessor&>(buf_acc_).access();
if (tbb == NULL)
{
UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer");
return -ErrLogic;
}
return tbb->read(offset, data, len);
}
virtual void registerInternalFailure(const char* msg)
{
internal_failure_cnt_++;
UAVCAN_TRACE("Node", "Internal failure: %s", msg);
#if UAVCAN_TINY
(void)msg;
#else
(void)getLogger().log(protocol::debug::LogLevel::ERROR, "UAVCAN", msg);
#endif
}
/**
* These methods set the value and then immediately read it back.
* 1. Serialize the value.
* 2. Update the value on the backend.
* 3. Call get() with the same value argument.
* The caller then is supposed to check whether the argument has the desired value.
*/
int setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value)
{
IStorageBackend::String serialized;
serialized.appendFormatted("%llu", static_cast<unsigned long long>(inout_value));
UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str());
storage_.set(key, serialized);
return get(key, inout_value);
}
TransferReceiver::ResultCode TransferReceiver::addFrame(const RxFrame& frame, TransferBufferAccessor& tba)
{
if ((frame.getMonotonicTimestamp().isZero()) ||
(frame.getMonotonicTimestamp() < prev_transfer_ts_) ||
(frame.getMonotonicTimestamp() < this_transfer_ts_))
{
return ResultNotComplete;
}
const bool not_initialized = !isInitialized();
const bool receiver_timed_out = isTimedOut(frame.getMonotonicTimestamp());
const bool same_iface = frame.getIfaceIndex() == iface_index_;
const bool first_fame = frame.isFirst();
const TidRelation tid_rel = getTidRelation(frame);
const bool iface_timed_out =
(frame.getMonotonicTimestamp() - this_transfer_ts_).toUSec() > (int64_t(transfer_interval_usec_) * 2);
// FSM, the hard way
const bool need_restart =
(not_initialized) ||
(receiver_timed_out) ||
(same_iface && first_fame && (tid_rel == TidFuture)) ||
(iface_timed_out && first_fame && (tid_rel == TidFuture));
if (need_restart)
{
const bool error = !not_initialized && !receiver_timed_out;
if (error)
{
registerError();
}
UAVCAN_TRACE("TransferReceiver",
"Restart [not_inited=%i, iface_timeout=%i, recv_timeout=%i, same_iface=%i, first_frame=%i, tid_rel=%i], %s",
int(not_initialized), int(iface_timed_out), int(receiver_timed_out), int(same_iface),
int(first_fame), int(tid_rel), frame.toString().c_str());
tba.remove();
iface_index_ = frame.getIfaceIndex();
tid_ = frame.getTransferID();
next_frame_index_ = 0;
buffer_write_pos_ = 0;
this_transfer_crc_ = 0;
if (!first_fame)
{
tid_.increment();
return ResultNotComplete;
}
}
if (!validate(frame))
{
return ResultNotComplete;
}
return receive(frame, tba);
}
void handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
{
if (msg.getTransferType() == TransferTypeMessageBroadcast)
{
processMsg(msg);
}
else
{
UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
}
}
virtual void handleTimerEvent(const TimerEvent&)
{
if (pending_nodes_.isEmpty())
{
TimerBase::stop();
UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped");
return;
}
const NodeID node_id = pickNextNodeID();
if (!node_id.isUnicast())
{
return;
}
FirmwareFilePath* const path = pending_nodes_.access(node_id);
if (path == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map
return;
}
protocol::file::BeginFirmwareUpdate::Request req;
req.source_node_id = getNode().getNodeID().get();
if (!common_path_prefix_.empty())
{
req.image_file_remote_path.path += common_path_prefix_.c_str();
req.image_file_remote_path.path.push_back(protocol::file::Path::SEPARATOR);
}
req.image_file_remote_path.path += path->c_str();
UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s",
int(node_id.get()), req.image_file_remote_path.path.c_str());
const int call_res = begin_fw_update_client_.call(node_id, req);
if (call_res < 0)
{
getNode().registerInternalFailure("FirmwareUpdateTrigger call");
}
}
void TransferBufferManager::remove(const TransferBufferManagerKey& key)
{
UAVCAN_ASSERT(!key.isEmpty());
TransferBufferManagerEntry* dyn = findFirst(key);
if (dyn != UAVCAN_NULLPTR)
{
UAVCAN_TRACE("TransferBufferManager", "Buffer deleted, %s", key.toString().c_str());
buffers_.remove(dyn);
TransferBufferManagerEntry::destroy(dyn, allocator_);
}
}
void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame)
{
RxFrame frame;
if (!frame.parse(can_frame))
{
UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str());
UAVCAN_ASSERT(0); // No way!
return;
}
UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID());
loopback_listeners_.invokeListeners(frame);
}
