libuavcan: Convenience method NodeStatusProvider::setStatusPublishingPeriod()

libuavcan/include/uavcan/protocol/node_status_provider.hpp

@@ -58,9 +58,6 @@ public:
         UAVCAN_ASSERT(!creation_timestamp_.isZero());
 
         node_info_.status.status_code = protocol::NodeStatus::STATUS_INITIALIZING;
-
-        // NodeStatus TX timeout equals its publication period minus some arbitrary time gap:
-        node_status_pub_.setTxTimeout(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS - 10));
     }
 
     /**
@@ -75,6 +72,14 @@ public:
      */
     int forcePublish() { return publish(); }
 
+    /**
+     * Allows to override default publishing rate for uavcan.protocol.NodeStatus.
+     * Refer to the DSDL definition of uavcan.protocol.NodeStatus to see what is the default rate.
+     * Doesn't fail; if the value is outside of acceptable range, a closest valid value will be used instead.
+     */
+    void setStatusPublishingPeriod(uavcan::MonotonicDuration period);
+    uavcan::MonotonicDuration getStatusPublishingPeriod() const;
+
     /**
      * Local node status code control.
      */

libuavcan/src/protocol/uc_node_status_provider.cpp

@@ -92,7 +92,7 @@ int NodeStatusProvider::startAndPublish()
         goto fail;
     }
 
-    TimerBase::startPeriodic(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS));
+    setStatusPublishingPeriod(MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS));
 
     return res;
 
@@ -104,6 +104,27 @@ fail:
     return res;
 }
 
+void NodeStatusProvider::setStatusPublishingPeriod(uavcan::MonotonicDuration period)
+{
+    const MonotonicDuration maximum = MonotonicDuration::fromMSec(protocol::NodeStatus::PUBLICATION_PERIOD_MS);
+    const MonotonicDuration minimum = MonotonicDuration::fromMSec(50);
+
+    period = min(period, maximum);
+    period = max(period, minimum);
+    TimerBase::startPeriodic(period);
+
+    const MonotonicDuration tx_timeout = period - MonotonicDuration::fromUSec(period.toUSec() / 20);
+    node_status_pub_.setTxTimeout(tx_timeout);
+
+    UAVCAN_TRACE("NodeStatusProvider", "Status pub period: %s, TX timeout: %s",
+                 period.toString().c_str(), node_status_pub_.getTxTimeout().toString().c_str());
+}
+
+uavcan::MonotonicDuration NodeStatusProvider::getStatusPublishingPeriod() const
+{
+    return TimerBase::getPeriod();
+}
+
 void NodeStatusProvider::setStatusCode(uint8_t code)
 {
     node_info_.status.status_code = code;

libuavcan/test/protocol/node_status_provider.cpp

@@ -46,6 +46,17 @@ TEST(NodeStatusProvider, Basic)
     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg3;
     ASSERT_LE(0, nsp.startAndPublish());
 
+    // Checking the publishing rate settings
+    ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::PUBLICATION_PERIOD_MS),
+              nsp.getStatusPublishingPeriod());
+
+    nsp.setStatusPublishingPeriod(uavcan::MonotonicDuration());
+    ASSERT_FALSE(nsp.getStatusPublishingPeriod().isZero());
+
+    nsp.setStatusPublishingPeriod(uavcan::MonotonicDuration::fromMSec(3600 * 1000 * 24));
+    ASSERT_EQ(uavcan::MonotonicDuration::fromMSec(uavcan::protocol::NodeStatus::PUBLICATION_PERIOD_MS),
+              nsp.getStatusPublishingPeriod());
+
     /*
      * Initial status publication
      */