global_time_sync_master.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED
00007 
00008 #include <uavcan/build_config.hpp>
00009 #include <uavcan/node/publisher.hpp>
00010 #include <uavcan/node/subscriber.hpp>
00011 #include <uavcan/util/method_binder.hpp>
00012 #include <uavcan/util/lazy_constructor.hpp>
00013 #include <uavcan/protocol/GlobalTimeSync.hpp>
00014 #include <uavcan/debug.hpp>
00015 #include <cstdlib>
00016 #include <cassert>
00017 
00018 namespace uavcan
00019 {
00020 /**
00021  * Please read the specs to learn how the time synchronization works.
00022  *
00023  * No more than one object of this class is allowed per node; otherwise a disaster is bound to happen.
00024  *
00025  * NOTE: In order for this class to work, the platform driver must implement
00026  *       CAN bus TX loopback with both UTC and monotonic timestamping.
00027  *
00028  * Ref. M. Gergeleit, H. Streich - "Implementing a Distributed High-Resolution Real-Time Clock using the CAN-Bus"
00029  *
00030  * TODO: Enforce max one master per node
00031  */
00032 class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
00033 {
00034     class IfaceMaster
00035     {
00036         Publisher<protocol::GlobalTimeSync>  pub_;
00037         MonotonicTime iface_prev_pub_mono_;
00038         UtcTime prev_tx_utc_;
00039         const uint8_t iface_index_;
00040 
00041     public:
00042         IfaceMaster(INode& node, uint8_t iface_index)
00043             : pub_(node)
00044             , iface_index_(iface_index)
00045         {
00046             UAVCAN_ASSERT(iface_index < MaxCanIfaces);
00047         }
00048 
00049         int init(TransferPriority priority)
00050         {
00051             const int res = pub_.init(priority);
00052             if (res >= 0)
00053             {
00054                 pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_));
00055                 pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback);
00056             }
00057             return res;
00058         }
00059 
00060         void setTxTimestamp(UtcTime ts)
00061         {
00062             if (ts.isZero())
00063             {
00064                 UAVCAN_ASSERT(0);
00065                 pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts");
00066                 return;
00067             }
00068             if (!prev_tx_utc_.isZero())
00069             {
00070                 prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it
00071                 pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict");
00072                 return;
00073             }
00074             prev_tx_utc_ = ts;
00075         }
00076 
00077         int publish(TransferID tid, MonotonicTime current_time)
00078         {
00079             UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback);
00080             UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_));
00081 
00082             const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_;
00083             iface_prev_pub_mono_ = current_time;
00084             UAVCAN_ASSERT(since_prev_pub.isPositive());
00085             const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS;
00086 
00087             protocol::GlobalTimeSync msg;
00088             msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec();
00089             prev_tx_utc_ = UtcTime();
00090 
00091             UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i",
00092                          static_cast<unsigned long long>(msg.previous_transmission_timestamp_usec),
00093                          int(iface_index_), int(tid.get()));
00094             return pub_.broadcast(msg, tid);
00095         }
00096     };
00097 
00098     INode& node_;
00099     LazyConstructor<IfaceMaster>  iface_masters_[MaxCanIfaces];
00100     MonotonicTime prev_pub_mono_;
00101     DataTypeID dtid_;
00102     bool initialized_;
00103 
00104     virtual void handleLoopbackFrame(const RxFrame& frame)
00105     {
00106         const uint8_t iface = frame.getIfaceIndex();
00107         if (initialized_ && iface < MaxCanIfaces)
00108         {
00109             if (frame.getDataTypeID() == dtid_ &&
00110                 frame.getTransferType() == TransferTypeMessageBroadcast &&
00111                 frame.isStartOfTransfer() && frame.isEndOfTransfer() &&
00112                 frame.getSrcNodeID() == node_.getNodeID())
00113             {
00114                 iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp());
00115             }
00116         }
00117         else
00118         {
00119             UAVCAN_ASSERT(0);
00120         }
00121     }
00122 
00123     int getNextTransferID(TransferID& tid)
00124     {
00125         const MonotonicDuration max_transfer_interval =
00126             MonotonicDuration::fromMSec(protocol::GlobalTimeSync::MAX_BROADCASTING_PERIOD_MS);
00127 
00128         const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast);
00129         const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval;
00130         TransferID* const tid_ptr =
00131             node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline);
00132         if (tid_ptr == UAVCAN_NULLPTR)
00133         {
00134             return -ErrMemory;
00135         }
00136 
00137         tid = *tid_ptr;
00138         tid_ptr->increment();
00139         return 0;
00140     }
00141 
00142 public:
00143     explicit GlobalTimeSyncMaster(INode& node)
00144         : LoopbackFrameListenerBase(node.getDispatcher())
00145         , node_(node)
00146         , initialized_(false)
00147     { }
00148 
00149     /**
00150      * Merely prepares the class to work, doesn't do anything else.
00151      * Must be called before the master can be used.
00152      * Returns negative error code.
00153      */
00154     int init(const TransferPriority priority = TransferPriority::OneLowerThanHighest)
00155     {
00156         if (initialized_)
00157         {
00158             return 0;
00159         }
00160 
00161         // Data type ID
00162         const DataTypeDescriptor* const desc =
00163             GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName());
00164         if (desc == UAVCAN_NULLPTR)
00165         {
00166             return -ErrUnknownDataType;
00167         }
00168         dtid_ = desc->getID();
00169 
00170         // Iface master array
00171         int res = -ErrLogic;
00172         for (uint8_t i = 0; i < MaxCanIfaces; i++)
00173         {
00174             if (!iface_masters_[i].isConstructed())
00175             {
00176                 iface_masters_[i].construct<INode&, uint8_t>(node_, i);
00177             }
00178             res = iface_masters_[i]->init(priority);
00179             if (res < 0)
00180             {
00181                 break;
00182             }
00183         }
00184 
00185         // Loopback listener
00186         initialized_ = res >= 0;
00187         if (initialized_)
00188         {
00189             LoopbackFrameListenerBase::startListening();
00190         }
00191         return res;
00192     }
00193 
00194     /**
00195      * Whether the master instance has been initialized.
00196      */
00197     bool isInitialized() const { return initialized_; }
00198 
00199     /**
00200      * Publishes one sync message.
00201      *
00202      * Every call to this method hints the master to publish the next sync message once. Exact time will
00203      * be obtained from the TX loopback timestamp field.
00204      *
00205      * This method shall be called with a proper interval - refer to the time sync message definition
00206      * for min/max interval values.
00207      *
00208      * Returns negative error code.
00209      */
00210     int publish()
00211     {
00212         if (!initialized_)
00213         {
00214             const int res = init();
00215             if (res < 0)
00216             {
00217                 return res;
00218             }
00219         }
00220 
00221         /*
00222          * Enforce max frequency
00223          */
00224         const MonotonicTime current_time = node_.getMonotonicTime();
00225         {
00226             const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_;
00227             UAVCAN_ASSERT(since_prev_pub.isPositive());
00228             if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_BROADCASTING_PERIOD_MS)
00229             {
00230                 UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped");
00231                 return 0;
00232             }
00233             prev_pub_mono_ = current_time;
00234         }
00235 
00236         /*
00237          * Obtain common Transfer ID for all masters
00238          */
00239         TransferID tid;
00240         {
00241             const int tid_res = getNextTransferID(tid);
00242             if (tid_res < 0)
00243             {
00244                 return tid_res;
00245             }
00246         }
00247 
00248         for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
00249         {
00250             const int res = iface_masters_[i]->publish(tid, current_time);
00251             if (res < 0)
00252             {
00253                 return res;
00254             }
00255         }
00256         return 0;
00257     }
00258 };
00259 
00260 }
00261 
00262 #endif // UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED