Skip to content
This repository has been archived by the owner on Sep 23, 2024. It is now read-only.

Commit

Permalink
Test: run heartbeat from IO loop.
Browse files Browse the repository at this point in the history
  • Loading branch information
@aentinger
aentinger committed Nov 17, 2023
1 parent d1eb073 commit 79b8c0b
Showing 1 changed file with 38 additions and 20 deletions.
58 changes: 38 additions & 20 deletions src/Node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,14 +66,32 @@ Node::Node()
_node_hdl.onCanFrameReceived(frame);
});

_cyphal_heartbeat_pub = _node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>(1*1000*1000UL /* = 1 sec in usecs. */);

_node_loop_timer = create_wall_timer(NODE_LOOP_RATE,
[this]()
{
auto const now = std::chrono::steady_clock::now();
std::lock_guard <std::mutex> lock(_node_mtx);

static auto prev_heartbeat = std::chrono::steady_clock::now();
if (now - prev_heartbeat > std::chrono::seconds(1))
{
uavcan::node::Heartbeat_1_0 msg;

msg.uptime = std::chrono::duration_cast<std::chrono::seconds>(
now - _node_start).count();
msg.health.value = uavcan::node::Health_1_0::NOMINAL;
msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL;
msg.vendor_specific_status_code = 0;

_cyphal_heartbeat_pub->publish(msg);
}

_node_hdl.spinSome();
});

init_cyphal_heartbeat();
// init_cyphal_heartbeat();
init_cyphal_node_info();

init_motor_left();
Expand All @@ -93,25 +111,25 @@ Node::~Node()

void Node::init_cyphal_heartbeat()
{
_cyphal_heartbeat_pub = _node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>(1*1000*1000UL /* = 1 sec in usecs. */);

_cyphal_heartbeat_timer = create_wall_timer(CYPHAL_HEARTBEAT_PERIOD,
[this]()
{
uavcan::node::Heartbeat_1_0 msg;

auto const now = std::chrono::steady_clock::now();

msg.uptime = std::chrono::duration_cast<std::chrono::seconds>(now - _node_start).count();
msg.health.value = uavcan::node::Health_1_0::NOMINAL;
msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL;
msg.vendor_specific_status_code = 0;

{
std::lock_guard <std::mutex> lock(_node_mtx);
_cyphal_heartbeat_pub->publish(msg);
}
});
// _cyphal_heartbeat_pub = _node_hdl.create_publisher<uavcan::node::Heartbeat_1_0>(1*1000*1000UL /* = 1 sec in usecs. */);

// _cyphal_heartbeat_timer = create_wall_timer(CYPHAL_HEARTBEAT_PERIOD,
// [this]()
// {
// uavcan::node::Heartbeat_1_0 msg;
//
// auto const now = std::chrono::steady_clock::now();
//
// msg.uptime = std::chrono::duration_cast<std::chrono::seconds>(now - _node_start).count();
// msg.health.value = uavcan::node::Health_1_0::NOMINAL;
// msg.mode.value = uavcan::node::Mode_1_0::OPERATIONAL;
// msg.vendor_specific_status_code = 0;
//
// {
// std::lock_guard <std::mutex> lock(_node_mtx);
// _cyphal_heartbeat_pub->publish(msg);
// }
// });
}

void Node::init_cyphal_node_info()
Expand Down

0 comments on commit 79b8c0b

Please sign in to comment.