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())); } }
static void handleMessage(Msg_t* msg) { GradientMsg_t* gradientMsg = ((GradientMsg_t*) msg->message); updateFromMsg(gradientMsg); }