Tutorial: Real-Time Control (Rust)

use horus::prelude::*;
use std::time::Instant;

struct RtMotorLoop {
    cmd: Topic<CmdVel>,     // motor.target — commanded RPM
    encoder: Topic<CmdVel>, // encoder.rpm  — measured RPM
    pwm: Topic<CmdVel>,     // motor.pwm    — duty cycle out

    target_rpm: f64,
    integral: f64,
    prev_error: f64,
    tick_count: u64,
    jitter_sum_us: u64,
    max_jitter_us: u64,
    min_jitter_us: u64,
    last_tick: Instant,
}

impl RtMotorLoop {
    fn new() -> Result<Self> {
        Ok(Self {
            cmd: Topic::new("motor.target")?,
            encoder: Topic::new("encoder.rpm")?,
            pwm: Topic::new("motor.pwm")?,
            target_rpm: 0.0,
            integral: 0.0,
            prev_error: 0.0,
            tick_count: 0,
            jitter_sum_us: 0,
            max_jitter_us: 0,
            min_jitter_us: u64::MAX,
            last_tick: Instant::now(),
        })
    }
}

impl Node for RtMotorLoop {
    fn name(&self) -> &str {
        "rt_motor_1khz"
    }

    fn init(&mut self) -> Result<()> {
        // Touch every ring buffer once before the deadline clock starts. The
        // first send/recv on a topic faults in its shared-memory pages, which
        // costs milliseconds — inside tick() that is a budget overrun on tick 1.
        self.pwm.send(CmdVel::new(0.0, 0.0));
        let _ = self.cmd.recv();
        let _ = self.encoder.recv();

        self.last_tick = Instant::now();
        hlog!(info, "1kHz RT loop initialized");
        Ok(())
    }

    fn tick(&mut self) {
        let now = Instant::now();
        let dt_us = now.duration_since(self.last_tick).as_micros() as u64;
        self.last_tick = now;

        // Track jitter statistics
        if self.tick_count > 10 {
            // skip first 10 ticks (startup)
            self.max_jitter_us = self.max_jitter_us.max(dt_us);
            self.min_jitter_us = self.min_jitter_us.min(dt_us);
            self.jitter_sum_us += dt_us;
        }
        self.tick_count += 1;

        // Read target velocity — drain to the newest, never queue up.
        while let Some(cmd) = self.cmd.recv() {
            self.target_rpm = cmd.linear as f64;
        }

        // Read actual RPM
        let mut actual_rpm = 0.0;
        while let Some(enc) = self.encoder.recv() {
            actual_rpm = enc.linear as f64;
        }

        // PID (tuned for motor dynamics)
        let error = self.target_rpm - actual_rpm;
        self.integral = (self.integral + error * 0.001).clamp(-100.0, 100.0); // dt = 1ms
        let derivative = (error - self.prev_error) * 1000.0;
        self.prev_error = error;

        let output =
            (0.5 * error + 0.01 * self.integral + 0.001 * derivative).clamp(-1.0, 1.0);
        self.pwm.send(CmdVel::new(output as f32, 0.0));

        // Report jitter every 1000 ticks (1 Hz)
        if self.tick_count % 1000 == 0 {
            let avg = self.jitter_sum_us as f64 / (self.tick_count - 10) as f64;
            hlog!(
                info,
                "jitter: min={}us avg={:.0}us max={}us (target=1000us)",
                self.min_jitter_us,
                avg,
                self.max_jitter_us
            );
        }
    }

    fn enter_safe_state(&mut self) {
        self.pwm.send(CmdVel::new(0.0, 0.0));
        self.integral = 0.0;

        hlog!(
            error,
            "SAFE STATE — max jitter was {}us",
            self.max_jitter_us
        );
    }
}

fn main() -> Result<()> {
    let mut sched = Scheduler::new()
        .tick_rate(1000_u64.hz())
        .name("rt_control")
        .blackbox(4) // 4 MB flight recorder for post-mortem analysis
        .prefer_rt();

    sched
        .add(RtMotorLoop::new()?)
        .order(0)
        .rate(1000_u64.hz())
        .budget(500_u64.us()) // must finish in 500us (50% of 1ms period)
        .deadline(900_u64.us()) // absolute deadline 900us
        .on_miss(Miss::SafeMode)
        .core(3) // dedicated CPU core
        .priority(95) // near-max SCHED_FIFO priority
        .watchdog(100_u64.ms()) // if stuck for 100ms, trigger safety
        .build()?;

    println!("1kHz RT motor loop starting...");
    println!("Monitor: horus log (see jitter stats)");
    sched.run()
}