Tutorial 5: Hardware & Real-Time (Rust)

use horus::prelude::*;
use std::fs::{File, OpenOptions};
use std::io::{Read, Write};
use std::os::unix::fs::OpenOptionsExt;
use std::process::Command;

// Linux open(2) flags. std does not re-export libc's constants and a HORUS
// project depends only on `horus`, so spell out the two we need.
const O_NOCTTY: i32 = 0o400;
const O_NONBLOCK: i32 = 0o4000;

// 100 ticks at 100 Hz = 1 s without encoder feedback.
const ENCODER_TIMEOUT_TICKS: u32 = 100;

struct MotorDriver {
    port: String,
    baudrate: u32,
    serial: Option<File>,
    cmd: Topic<CmdVel>,
    state: Topic<CmdVel>,
    last_cmd: f32,
    watchdog_counter: u32,
    watchdog_fed: bool,
    watchdog_alarmed: bool,
}

impl MotorDriver {
    fn new(port: &str, baudrate: u32) -> Result<Self> {
        Ok(Self {
            port: port.to_string(),
            baudrate,
            serial: None,
            cmd: Topic::new("motor.cmd")?,
            state: Topic::new("motor.state")?,
            last_cmd: 0.0,
            watchdog_counter: 0,
            watchdog_fed: false,
            watchdog_alarmed: false,
        })
    }

    /// Write one framed command to the motor controller.
    /// Protocol: "M<duty>\n" where duty is -100 to 100.
    fn write_duty(&mut self, duty: f32) {
        // Format into a stack buffer: the hot path of an RT tick should not
        // reach the heap.
        let mut buf = [0u8; 32];
        let used = {
            let mut cursor = &mut buf[..];
            let _ = write!(cursor, "M{:.0}\n", duty);
            32 - cursor.len()
        };
        if let Some(serial) = self.serial.as_mut() {
            let _ = serial.write_all(&buf[..used]);
        }
    }

    fn send_zero(&mut self) {
        self.write_duty(0.0);
        self.state.send(CmdVel::new(0.0, 0.0));
    }
}

impl Node for MotorDriver {
    fn name(&self) -> &str {
        "motor_driver"
    }

    fn init(&mut self) -> Result<()> {
        // Line discipline: 115200 8N1, raw, no modem control.
        let stty = Command::new("stty")
            .arg("-F")
            .arg(&self.port)
            .arg(self.baudrate.to_string())
            .args(["raw", "-echo", "clocal", "cs8", "-parenb", "-cstopb"])
            .status();
        if let Err(e) = stty {
            hlog!(warn, "stty failed on {}: {}", self.port, e);
        }

        match OpenOptions::new()
            .read(true)
            .write(true)
            .custom_flags(O_NOCTTY | O_NONBLOCK)
            .open(&self.port)
        {
            Ok(serial) => {
                self.serial = Some(serial);
                hlog!(info, "Serial port {} opened, motor ready", self.port);
            }
            Err(e) => {
                // Log rather than fail: the scheduler keeps ticking and every
                // tick short-circuits while `serial` is None.
                hlog!(error, "Failed to open serial port {}: {}", self.port, e);
            }
        }
        Ok(())
    }

    fn tick(&mut self) {
        if self.serial.is_none() {
            return;
        }

        // Drain to the newest command so a burst never queues up behind us.
        let mut latest = None;
        while let Some(cmd) = self.cmd.recv() {
            latest = Some(cmd);
        }
        if let Some(cmd) = latest {
            self.last_cmd = cmd.linear;
            self.write_duty(cmd.linear * 100.0);
        }

        // Encoder feedback. The fd is O_NONBLOCK, so an idle UART returns
        // WouldBlock immediately instead of parking the RT thread.
        let mut rbuf = [0u8; 64];
        let n = match self.serial.as_mut() {
            Some(serial) => serial.read(&mut rbuf).unwrap_or(0),
            None => 0,
        };
        if n > 0 {
            if let Ok(text) = std::str::from_utf8(&rbuf[..n]) {
                let rpm = text
                    .trim()
                    .strip_prefix('E')
                    .and_then(|v| v.parse::<f32>().ok());
                if let Some(rpm) = rpm {
                    self.state.send(CmdVel::new(rpm, self.last_cmd));
                    self.watchdog_fed = true;
                }
            }
        }

        // Watchdog: no encoder response for 100 ticks (1 s) means the cable is
        // out, or the motor controller has stopped talking.
        if self.watchdog_fed {
            self.watchdog_counter = 0;
            self.watchdog_fed = false;
            // Clear the latch so a recovered encoder can alarm again later.
            self.watchdog_alarmed = false;
        } else {
            self.watchdog_counter += 1;
            if self.watchdog_counter > ENCODER_TIMEOUT_TICKS && !self.watchdog_alarmed {
                hlog!(error, "Encoder watchdog timeout (no feedback for 1 s)");
                self.watchdog_alarmed = true;
                self.send_zero();
            }
        }
    }

    fn enter_safe_state(&mut self) {
        self.send_zero();
        hlog!(error, "Safe state: motor zeroed");
    }

    fn shutdown(&mut self) -> Result<()> {
        if self.serial.is_some() {
            self.send_zero();
            self.serial = None; // dropping the File closes the fd
        }
        hlog!(info, "Serial port closed");
        Ok(())
    }
}

fn main() -> Result<()> {
    let mut sched = Scheduler::new()
        .tick_rate(100_u64.hz())
        .name("rt_motor")
        .blackbox(8) // 8 MB flight recorder for post-mortem analysis
        .prefer_rt(); // use SCHED_FIFO if available

    sched
        .add(MotorDriver::new("/dev/ttyUSB0", 115_200)?)
        .order(0) // highest priority
        .budget(2_u64.ms()) // must complete in 2 ms
        .deadline(5_u64.ms()) // absolute deadline 5 ms
        .on_miss(Miss::SafeMode) // stop motor if overrun
        .core(2) // pin to CPU core 2
        .priority(90) // SCHED_FIFO priority 90
        .watchdog(1_u64.secs()) // scheduler-level watchdog
        .build()?;

    sched.run()
}

horus build && horus run

use horus::prelude::*;

fn main() -> Result<()> {
    // Try SCHED_FIFO and mlockall; log a warning and keep going if the kernel
    // or your privileges do not allow them. Never panics.
    let sched = Scheduler::new().tick_rate(1000_u64.hz()).prefer_rt();

    // Production alternative — panics at construction instead of running
    // without RT:
    //     let sched = Scheduler::new().tick_rate(1000_u64.hz()).require_rt();

    // prefer_rt() succeeds even when nothing was granted, so ask afterwards.
    if !sched.has_full_rt() {
        for degradation in sched.degradations() {
            hlog!(warn, "RT degraded: {:?}", degradation);
        }
    }
    Ok(())
}

# Check current kernel
uname -r  # Look for "-rt" suffix

# Let HORUS inspect and configure the machine for you
horus setup-rt --check
horus setup-rt

# Or set the CPU governor to performance by hand
sudo cpupower frequency-set -g performance

# Grant RT privileges without root (release build, not the debug one)
horus build --release
sudo setcap cap_sys_nice+ep target/release/rt_motor

# Or run with elevated privileges
sudo nice -n -20 target/release/rt_motor