use crate::{Node, PublishError, StartSendError};
use canadensis_core::time::MicrosecondDuration32;
use canadensis_core::transport::Transmitter;
use canadensis_core::{nb, Priority};
use canadensis_data_types::uavcan::node::health_1_0::Health;
use canadensis_data_types::uavcan::node::heartbeat_1_0::{self, Heartbeat};
use canadensis_data_types::uavcan::node::mode_1_0::Mode;
pub struct MinimalNode<N>
where
N: Node,
{
node: N,
heartbeat: Heartbeat,
}
impl<N> MinimalNode<N>
where
N: Node,
{
pub fn new(
mut node: N,
) -> Result<Self, StartSendError<<N::Transmitter as Transmitter<N::Clock>>::Error>> {
let heartbeat = Heartbeat {
uptime: 0,
health: Health {
value: Health::NOMINAL,
},
mode: Mode {
value: Mode::OPERATIONAL,
},
vendor_specific_status_code: 0,
};
let heartbeat_timeout = MicrosecondDuration32::from_ticks(500_000);
node.start_publishing(
heartbeat_1_0::SUBJECT,
heartbeat_timeout,
Priority::Nominal.into(),
)?;
Ok(MinimalNode { node, heartbeat })
}
pub fn run_per_second_tasks(
&mut self,
) -> nb::Result<(), PublishError<<N::Transmitter as Transmitter<N::Clock>>::Error>> {
self.send_heartbeat()
}
fn send_heartbeat(
&mut self,
) -> nb::Result<(), PublishError<<N::Transmitter as Transmitter<N::Clock>>::Error>> {
self.heartbeat.uptime = self.heartbeat.uptime.saturating_add(1);
self.node.publish(heartbeat_1_0::SUBJECT, &self.heartbeat)
}
pub fn set_mode(&mut self, mode: Mode) {
self.heartbeat.mode = mode;
}
pub fn set_health(&mut self, health: Health) {
self.heartbeat.health = health;
}
pub fn set_status_code(&mut self, status: u8) {
self.heartbeat.vendor_specific_status_code = status;
}
pub fn node(&self) -> &N {
&self.node
}
pub fn node_mut(&mut self) -> &mut N {
&mut self.node
}
}