Tutorial 10: Write a Reusable Driver (Rust)

use horus::hardware::NodeParams;
use horus::prelude::*;
use horus::register_driver;
use std::fs::{File, OpenOptions};
use std::io::Read;
use std::os::unix::fs::OpenOptionsExt;

const TICK_HZ: u64 = 100;
const ERROR_LIMIT: u32 = 100; // 1 second at 100 Hz
const HEARTBEAT_EVERY: u64 = 100; // 1 Hz
const STATUS_EVERY: u64 = 500; // 0.2 Hz

// O_NONBLOCK as open(2) defines it on Linux. A generated HORUS project depends
// only on `horus`, so the flag is spelled out here rather than pulled in from
// `libc`; a driver that already has a serial crate should use its constant.
const O_NONBLOCK: i32 = 0o4000;

// ── The driver ──────────────────────────────────────────────────────────
struct ImuDriver {
    params: RuntimeParams,
    imu: Topic<Imu>,
    heartbeat: Topic<Heartbeat>,
    status: Topic<DiagnosticStatus>,

    port: Option<File>,
    connected: bool,
    ticks: u64,
    consecutive_errors: u32,
    gyro_bias_z: f64,
}

impl ImuDriver {
    // `RuntimeParams` is a shared handle (cloning it shares the same store), so
    // the driver keeps its own clone and reads through it whenever it likes.
    fn new(params: RuntimeParams) -> Result<Self> {
        Ok(Self {
            params,
            imu: Topic::new("imu.data")?,
            heartbeat: Topic::new("imu.heartbeat")?,
            status: Topic::new("imu.status")?,
            port: None,
            connected: false,
            ticks: 0,
            consecutive_errors: 0,
            gyro_bias_z: 0.0,
        })
    }

    // The second constructor: build from a `[hardware.imu]` table in
    // horus.toml. It maps the table's keys onto the same parameter names the
    // driver reads, so both routes configure exactly one thing.
    fn from_params(cfg: &NodeParams) -> Result<Self> {
        let params = RuntimeParams::new()?;
        params.set("imu.port", cfg.get_or("port", "/dev/ttyUSB0".to_string()))?;
        params.set("imu.baudrate", cfg.get_or("baudrate", 115200_u64))?;
        params.set("imu.calibrate", cfg.get_or("calibrate", true))?;
        Self::new(params)
    }

    fn publish_status(&mut self, level: StatusLevel, message: &str) {
        self.status
            .send(DiagnosticStatus::new(level, 0, message).with_component("imu_driver"));
    }

    fn calibrate(&mut self) {
        hlog!(info, "Calibrating (hold still)...");
        // A real driver averages ~100 gyro samples while the robot is still and
        // keeps the mean as the bias.
        self.gyro_bias_z = 0.0;
        hlog!(info, "Calibration complete");
        self.publish_status(StatusLevel::Ok, "Ready");
    }

    fn parse_and_publish(&mut self, _frame: &[u8]) {
        // Parse the device-specific protocol into the standard Imu message.
        // Imu::new() starts from an identity quaternion and stamps the message,
        // so only the live axes are assigned here.
        let mut imu = Imu::new();
        imu.angular_velocity = [0.0, 0.0, 0.01];
        imu.linear_acceleration = [0.0, 0.0, 9.81];

        // Apply the calibration offset measured at startup.
        imu.angular_velocity[2] -= self.gyro_bias_z;

        self.imu.send(imu);
    }

    fn close_port(&mut self) {
        // Dropping the File closes the descriptor.
        self.port = None;
        self.connected = false;
    }
}

impl Node for ImuDriver {
    fn name(&self) -> &str {
        "imu_driver"
    }

    fn init(&mut self) -> Result<()> {
        let port: String = self.params.get_or("imu.port", "/dev/ttyUSB0".to_string());
        let baudrate: u64 = self.params.get_or("imu.baudrate", 115200_u64);

        // O_NONBLOCK is the flag that matters: tick() must never block the
        // scheduler waiting on a device that has stopped talking.
        match OpenOptions::new()
            .read(true)
            .write(true)
            .custom_flags(O_NONBLOCK)
            .open(&port)
        {
            Ok(file) => {
                self.port = Some(file);
                self.connected = true;
                hlog!(info, "IMU connected on {} at {} baud", port, baudrate);
                self.publish_status(StatusLevel::Ok, "Connected and calibrating");
            }
            Err(e) => {
                hlog!(error, "Could not open {}: {}", port, e);
                self.publish_status(StatusLevel::Error, "Failed to open serial port");
                // Returning Ok keeps the node in the schedule: it reports
                // "Disconnected" on imu.status rather than taking the whole
                // process down because one cable is out.
                return Ok(());
            }
        }

        if self.params.get_or("imu.calibrate", true) {
            self.calibrate();
        }
        Ok(())
    }

    fn tick(&mut self) {
        self.ticks += 1;

        if self.connected {
            let mut buf = [0u8; 64];
            let read_result = match self.port.as_mut() {
                Some(file) => file.read(&mut buf),
                None => Ok(0),
            };

            match read_result {
                Ok(n) if n > 0 => {
                    self.parse_and_publish(&buf[..n]);
                    self.consecutive_errors = 0;
                }
                _ => {
                    // Both an empty read (WouldBlock, no frame yet) and a hard
                    // I/O failure count towards the same limit — one second of
                    // silence from an IMU is a dead IMU either way.
                    self.consecutive_errors += 1;
                    if self.consecutive_errors > ERROR_LIMIT {
                        hlog!(error, "IMU read timeout — {} consecutive misses", ERROR_LIMIT);
                        self.publish_status(StatusLevel::Error, "Read timeout");
                        self.close_port();
                    }
                }
            }
        }

        // Heartbeat at 1 Hz.
        if self.ticks % HEARTBEAT_EVERY == 0 {
            let mut hb = Heartbeat::new("imu_driver", 0);
            hb.sequence = self.ticks / HEARTBEAT_EVERY;
            hb.alive = self.connected as u8;
            hb.uptime = self.ticks as f64 / TICK_HZ as f64;
            self.heartbeat.send(hb);
        }

        // Status at 0.2 Hz.
        if self.ticks % STATUS_EVERY == 0 {
            if self.connected {
                self.publish_status(StatusLevel::Ok, "OK");
            } else {
                self.publish_status(StatusLevel::Error, "Disconnected");
            }
        }
    }

    fn enter_safe_state(&mut self) {
        hlog!(warn, "Safe state — closing device");
        self.close_port();
    }

    fn shutdown(&mut self) -> Result<()> {
        self.close_port();
        hlog!(info, "IMU driver stopped");
        Ok(())
    }
}

// Makes the driver loadable from a [hardware.imu] table in horus.toml.
register_driver!(ImuDriver, ImuDriver::from_params);

fn main() -> Result<()> {
    let params = RuntimeParams::new()?;
    params.set("imu.port", "/dev/ttyUSB0")?;
    params.set("imu.baudrate", 115200_u64)?;
    params.set("imu.calibrate", true)?;

    let mut sched = Scheduler::new()
        .tick_rate(100_u64.hz())
        .name("imu_node")
        .prefer_rt()
        .blackbox(16) // flight recorder for crash forensics
        .with_params(params.clone());

    sched
        .add(ImuDriver::new(params)?)
        .order(0)
        .budget(2_u64.ms())
        .on_miss(Miss::Warn)
        .watchdog(5_u64.secs())
        .build()?;

    println!("IMU driver running at 100 Hz (Ctrl+C to stop)");
    sched.run()
}

// In another project's main.rs
sched
    .add(ImuDriver::new(params.clone())?)
    .order(0)
    .budget(2_u64.ms())
    .build()?;

[hardware.imu]
use       = "ImuDriver"   # the type name passed to register_driver!
port      = "/dev/ttyUSB0"
baudrate  = 115200
calibrate = true

use horus::prelude::*;

fn main() -> Result<()> {
    let mut sched = Scheduler::new().tick_rate(100_u64.hz()).name("robot");

    // hardware::load() reads the [hardware] tables out of horus.toml and
    // returns Vec<(String, Box<dyn Node>)> — one entry per declared device.
    // Nothing in the CLI does this for you; the program has to ask.
    for (name, node) in horus::hardware::load()? {
        hlog!(info, "loading hardware node {}", name);
        sched
            .add(node)
            .order(0)
            .budget(2_u64.ms())
            .on_miss(Miss::Warn)
            .build()?;
    }

    sched.run()
}

use horus::prelude::*;

struct ImuHealth {
    heartbeat: Topic<Heartbeat>,
    status: Topic<DiagnosticStatus>,
    ticks_since_heartbeat: u32,
}

impl ImuHealth {
    fn new() -> Result<Self> {
        Ok(Self {
            heartbeat: Topic::new("imu.heartbeat")?,
            status: Topic::new("imu.status")?,
            ticks_since_heartbeat: 0,
        })
    }
}

impl Node for ImuHealth {
    fn name(&self) -> &str {
        "imu_health"
    }

    fn tick(&mut self) {
        self.ticks_since_heartbeat += 1;

        while let Some(hb) = self.heartbeat.recv() {
            self.ticks_since_heartbeat = 0;
            // The driver is running but the device is gone.
            if hb.alive == 0 {
                hlog!(warn, "IMU reports itself not alive (seq {})", hb.sequence);
            }
        }

        while let Some(st) = self.status.recv() {
            if st.level >= StatusLevel::Error as u8 {
                hlog!(error, "IMU status: {}", st.message_str());
            }
        }

        // Two missed beats at 1 Hz means the driver itself is gone — a
        // different failure from `alive == 0`, and one no `imu.data` timeout
        // can tell apart from a merely idle sensor.
        if self.ticks_since_heartbeat > 200 {
            hlog!(error, "No IMU heartbeat for 2 s");
            self.ticks_since_heartbeat = 0;
        }
    }
}