global_time_sync_slave.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */
 
#ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
#define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
 
#include <uavcan/node/subscriber.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/protocol/GlobalTimeSync.hpp>
#include <uavcan/debug.hpp>
#include <cassert>
 
namespace uavcan
{
class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable
{
    typedef MethodBinder<GlobalTimeSyncSlave*,
                         void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure<protocol::GlobalTimeSync>&)>
        GlobalTimeSyncCallback;
 
    Subscriber<protocol::GlobalTimeSync, GlobalTimeSyncCallback> sub_;
 
    UtcTime prev_ts_utc_;
    MonotonicTime prev_ts_mono_;
    MonotonicTime last_adjustment_ts_;
    enum State { Update, Adjust } state_;
    NodeID master_nid_;
    TransferID prev_tid_;
    uint8_t prev_iface_index_;
    bool suppressed_;
 
    ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
 
    void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
    {
        UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0);
        const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_;
 
        UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i",
                     static_cast<long long>(adjustment.toUSec()),
                     int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_));
 
        if (!suppressed_)
        {
            getSystemClock().adjustUtc(adjustment);
        }
        last_adjustment_ts_ = msg.getMonotonicTimestamp();
        state_ = Update;
    }
 
    void updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
    {
        UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i",
                     int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
 
        prev_ts_utc_      = msg.getUtcTimestamp();
        prev_ts_mono_     = msg.getMonotonicTimestamp();
        master_nid_       = msg.getSrcNodeID();
        prev_iface_index_ = msg.getIfaceIndex();
        prev_tid_         = msg.getTransferID();
        state_            = Adjust;
    }
 
    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 handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
    {
        if (msg.getTransferType() == TransferTypeMessageBroadcast)
        {
            processMsg(msg);
        }
        else
        {
            UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
        }
    }
 
public:
    explicit GlobalTimeSyncSlave(INode& node)
        : sub_(node)
        , state_(Update)
        , prev_iface_index_(0xFF)
        , suppressed_(false)
    { }
 
    int start()
    {
        return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
    }
 
    void suppress(bool suppressed) { suppressed_ = suppressed; }
    bool isSuppressed() const { return suppressed_; }
 
    bool isActive() const
    {
        const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
        return !last_adjustment_ts_.isZero() &&
               (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS);
    }
 
    NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); }
 
    MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; }
};
 
}
 
#endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED