uc_node_status_provider.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/protocol/node_status_provider.hpp>
00006 #include <uavcan/debug.hpp>
00007 #include <cassert>
00008 
00009 namespace uavcan
00010 {
00011 
00012 bool NodeStatusProvider::isNodeInfoInitialized() const
00013 {
00014     // Hardware/Software versions are not required
00015     return !node_info_.name.empty();
00016 }
00017 
00018 int NodeStatusProvider::publish()
00019 {
00020     const MonotonicDuration uptime = getNode().getMonotonicTime() - creation_timestamp_;
00021     UAVCAN_ASSERT(uptime.isPositive());
00022     node_info_.status.uptime_sec = uint32_t(uptime.toMSec() / 1000);
00023 
00024     UAVCAN_ASSERT(node_info_.status.health <= protocol::NodeStatus::FieldTypes::health::max());
00025 
00026     return node_status_pub_.broadcast(node_info_.status);
00027 }
00028 
00029 void NodeStatusProvider::handleTimerEvent(const TimerEvent&)
00030 {
00031     if (getNode().isPassiveMode())
00032     {
00033         UAVCAN_TRACE("NodeStatusProvider", "NodeStatus pub skipped - passive mode");
00034     }
00035     else
00036     {
00037         if (ad_hoc_status_updater_ != UAVCAN_NULLPTR)
00038         {
00039             ad_hoc_status_updater_->updateNodeStatus();
00040         }
00041 
00042         const int res = publish();
00043         if (res < 0)
00044         {
00045             getNode().registerInternalFailure("NodeStatus pub failed");
00046         }
00047     }
00048 }
00049 
00050 void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::Request&,
00051                                                   protocol::GetNodeInfo::Response& rsp)
00052 {
00053     UAVCAN_TRACE("NodeStatusProvider", "Got GetNodeInfo request");
00054     UAVCAN_ASSERT(isNodeInfoInitialized());
00055     rsp = node_info_;
00056 }
00057 
00058 int NodeStatusProvider::startAndPublish(const TransferPriority priority)
00059 {
00060     if (!isNodeInfoInitialized())
00061     {
00062         UAVCAN_TRACE("NodeStatusProvider", "Node info was not initialized");
00063         return -ErrNotInited;
00064     }
00065 
00066     int res = -1;
00067 
00068     node_status_pub_.setPriority(priority);
00069 
00070     if (!getNode().isPassiveMode())
00071     {
00072         res = publish();
00073         if (res < 0)  // Initial broadcast
00074         {
00075             goto fail;
00076         }
00077     }
00078 
00079     res = gni_srv_.start(GetNodeInfoCallback(this, &NodeStatusProvider::handleGetNodeInfoRequest));
00080     if (res < 0)
00081     {
00082         goto fail;
00083     }
00084 
00085     setStatusPublicationPeriod(MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS));
00086 
00087     return res;
00088 
00089 fail:
00090     UAVCAN_ASSERT(res < 0);
00091     gni_srv_.stop();
00092     TimerBase::stop();
00093     return res;
00094 }
00095 
00096 void NodeStatusProvider::setStatusPublicationPeriod(uavcan::MonotonicDuration period)
00097 {
00098     const MonotonicDuration maximum = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_BROADCASTING_PERIOD_MS);
00099     const MonotonicDuration minimum = MonotonicDuration::fromMSec(protocol::NodeStatus::MIN_BROADCASTING_PERIOD_MS);
00100 
00101     period = min(period, maximum);
00102     period = max(period, minimum);
00103     TimerBase::startPeriodic(period);
00104 
00105     const MonotonicDuration tx_timeout = period - MonotonicDuration::fromUSec(period.toUSec() / 20);
00106     node_status_pub_.setTxTimeout(tx_timeout);
00107 
00108     UAVCAN_TRACE("NodeStatusProvider", "Status pub period: %s, TX timeout: %s",
00109                  period.toString().c_str(), node_status_pub_.getTxTimeout().toString().c_str());
00110 }
00111 
00112 uavcan::MonotonicDuration NodeStatusProvider::getStatusPublicationPeriod() const
00113 {
00114     return TimerBase::getPeriod();
00115 }
00116 
00117 void NodeStatusProvider::setAdHocNodeStatusUpdater(IAdHocNodeStatusUpdater* updater)
00118 {
00119     ad_hoc_status_updater_ = updater;   // Can be nullptr, that's okay
00120 }
00121 
00122 void NodeStatusProvider::setHealth(uint8_t code)
00123 {
00124     node_info_.status.health = code;
00125 }
00126 
00127 void NodeStatusProvider::setMode(uint8_t code)
00128 {
00129     node_info_.status.mode = code;
00130 }
00131 
00132 void NodeStatusProvider::setVendorSpecificStatusCode(VendorSpecificStatusCode code)
00133 {
00134     node_info_.status.vendor_specific_status_code = code;
00135 }
00136 
00137 void NodeStatusProvider::setName(const NodeName& name)
00138 {
00139     if (node_info_.name.empty())
00140     {
00141         node_info_.name = name;
00142     }
00143 }
00144 
00145 void NodeStatusProvider::setSoftwareVersion(const protocol::SoftwareVersion& version)
00146 {
00147     if (node_info_.software_version == protocol::SoftwareVersion())
00148     {
00149         node_info_.software_version = version;
00150     }
00151 }
00152 
00153 void NodeStatusProvider::setHardwareVersion(const protocol::HardwareVersion& version)
00154 {
00155     if (node_info_.hardware_version == protocol::HardwareVersion())
00156     {
00157         node_info_.hardware_version = version;
00158     }
00159 }
00160 
00161 }