diff --git a/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/libuavcan/include/uavcan/protocol/node_status_provider.hpp index cfded34a1f..22f6eb2f80 100644 --- a/libuavcan/include/uavcan/protocol/node_status_provider.hpp +++ b/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. */ diff --git a/libuavcan/src/protocol/uc_node_status_provider.cpp b/libuavcan/src/protocol/uc_node_status_provider.cpp index c886bdc669..998564fa9e 100644 --- a/libuavcan/src/protocol/uc_node_status_provider.cpp +++ b/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; diff --git a/libuavcan/test/protocol/node_status_provider.cpp b/libuavcan/test/protocol/node_status_provider.cpp index 52b4ae2b93..e6185e5d19 100644 --- a/libuavcan/test/protocol/node_status_provider.cpp +++ b/libuavcan/test/protocol/node_status_provider.cpp @@ -46,6 +46,17 @@ TEST(NodeStatusProvider, Basic) uavcan::DefaultDataTypeRegistrator _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 */