global_time_sync_slave.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED
00007 
00008 #include <uavcan/node/subscriber.hpp>
00009 #include <uavcan/util/method_binder.hpp>
00010 #include <uavcan/protocol/GlobalTimeSync.hpp>
00011 #include <uavcan/debug.hpp>
00012 #include <cassert>
00013 
00014 namespace uavcan
00015 {
00016 /**
00017  * Please read the specs to learn how the time synchronization works.
00018  *
00019  * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen.
00020  *
00021  * NOTE: In order for this class to work, the platform driver must implement:
00022  *  - CAN bus RX UTC timestamping;
00023  *  - Clock adjustment method in the system clock interface @ref ISystemClock::adjustUtc().
00024  *
00025  * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus"
00026  * http://modecs.cs.uni-salzburg.at/results/related_documents/CAN_clock.pdf
00027  */
00028 class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable
00029 {
00030     typedef MethodBinder<GlobalTimeSyncSlave*,
00031                          void (GlobalTimeSyncSlave::*)(const ReceivedDataStructure<protocol::GlobalTimeSync>&)>
00032         GlobalTimeSyncCallback;
00033 
00034     Subscriber<protocol::GlobalTimeSync, GlobalTimeSyncCallback> sub_;
00035 
00036     UtcTime prev_ts_utc_;
00037     MonotonicTime prev_ts_mono_;
00038     MonotonicTime last_adjustment_ts_;
00039     enum State { Update, Adjust } state_;
00040     NodeID master_nid_;
00041     TransferID prev_tid_;
00042     uint8_t prev_iface_index_;
00043     bool suppressed_;
00044 
00045     ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
00046 
00047     void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
00048     {
00049         UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0);
00050         const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_;
00051 
00052         UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i",
00053                      static_cast<long long>(adjustment.toUSec()),
00054                      int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_));
00055 
00056         if (!suppressed_)
00057         {
00058             getSystemClock().adjustUtc(adjustment);
00059         }
00060         last_adjustment_ts_ = msg.getMonotonicTimestamp();
00061         state_ = Update;
00062     }
00063 
00064     void updateFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
00065     {
00066         UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i",
00067                      int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
00068 
00069         prev_ts_utc_      = msg.getUtcTimestamp();
00070         prev_ts_mono_     = msg.getMonotonicTimestamp();
00071         master_nid_       = msg.getSrcNodeID();
00072         prev_iface_index_ = msg.getIfaceIndex();
00073         prev_tid_         = msg.getTransferID();
00074         state_            = Adjust;
00075     }
00076 
00077     void processMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
00078     {
00079         const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_;
00080         UAVCAN_ASSERT(!since_prev_msg.isNegative());
00081 
00082         const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero();
00083         const bool switch_master = msg.getSrcNodeID() < master_nid_;
00084         // TODO: Make configurable
00085         const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS;
00086 
00087         if (switch_master || pub_timeout || needs_init)
00088         {
00089             UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i",
00090                          int(needs_init), int(switch_master), int(pub_timeout));
00091             updateFromMsg(msg);
00092         }
00093         else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_)
00094         {
00095             if (state_ == Adjust)
00096             {
00097                 const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0;
00098                 const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1;
00099                 const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS;
00100                 if (msg_invalid || wrong_tid || wrong_timing)
00101                 {
00102                     UAVCAN_TRACE("GlobalTimeSyncSlave",
00103                                  "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i",
00104                                  int(msg_invalid), int(wrong_tid), int(wrong_timing));
00105                     state_ = Update;
00106                 }
00107             }
00108             if (state_ == Adjust)
00109             {
00110                 adjustFromMsg(msg);
00111             }
00112             else
00113             {
00114                 updateFromMsg(msg);
00115             }
00116         }
00117         else
00118         {
00119             UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i",
00120                          int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
00121         }
00122     }
00123 
00124     void handleGlobalTimeSync(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg)
00125     {
00126         if (msg.getTransferType() == TransferTypeMessageBroadcast)
00127         {
00128             processMsg(msg);
00129         }
00130         else
00131         {
00132             UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType()));
00133         }
00134     }
00135 
00136 public:
00137     explicit GlobalTimeSyncSlave(INode& node)
00138         : sub_(node)
00139         , state_(Update)
00140         , prev_iface_index_(0xFF)
00141         , suppressed_(false)
00142     { }
00143 
00144     /**
00145      * Starts the time sync slave. Once started, it works on its own and does not require any
00146      * attention from the application, other than to handle a clock adjustment request occasionally.
00147      * Returns negative error code.
00148      */
00149     int start()
00150     {
00151         return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
00152     }
00153 
00154     /**
00155      * Enable or disable the suppressed mode.
00156      *
00157      * In suppressed mode the slave will continue tracking time sync masters in the network, but will not
00158      * perform local clock adjustments. So it's kind of a dry run - all the time sync logic works except
00159      * the local clock will not receive adjustments.
00160      *
00161      * Suppressed mode is useful for nodes that can act as a back-up clock sync masters - as long as the
00162      * node sees a higher priority time sync master in the network, its slave will be NOT suppressed
00163      * in order to sync the local clock with the global master. As soon as all other higher priority
00164      * masters go down, the local node will suppress its time sync slave instance and become a new master.
00165      *
00166      * Suppressed mode is disabled by default.
00167      */
00168     void suppress(bool suppressed) { suppressed_ = suppressed; }
00169     bool isSuppressed() const { return suppressed_; }
00170 
00171     /**
00172      * If the clock sync slave sees any clock sync masters in the network, it is ACTIVE.
00173      * When the last master times out (PUBLISHER_TIMEOUT), the slave will be INACTIVE.
00174      * Note that immediately after start up the slave will be INACTIVE until it finds a master.
00175      * Please read the specs to learn more.
00176      */
00177     bool isActive() const
00178     {
00179         const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
00180         return !last_adjustment_ts_.isZero() &&
00181                (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::RECOMMENDED_BROADCASTER_TIMEOUT_MS);
00182     }
00183 
00184     /**
00185      * Node ID of the master the slave is currently locked on.
00186      * Returns an invalid Node ID if there's no active master.
00187      */
00188     NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); }
00189 
00190     /**
00191      * Last time when the local clock adjustment was performed.
00192      */
00193     MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; }
00194 };
00195 
00196 }
00197 
00198 #endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_SLAVE_HPP_INCLUDED