fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 20:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

libuavcan: Convenience method NodeStatusProvider::setStatusPublishingPeriod()

Browse Source
This commit is contained in:
Pavel Kirienko
2014-08-24 19:11:14 +04:00
parent 75153eb643
commit 6e3627dda8
3 changed files with 41 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/src/protocol/uc_node_status_provider.cpp
+22 -1
View File
@@ -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;