Tutorial: Real-Time Control (C++)

Build a 1 kHz motor control loop — the kind that runs real industrial servos. Measure jitter, enforce deadlines, and prove your system meets timing requirements.

What You'll Build

A control loop that:

  • Runs at exactly 1000 Hz
  • Reads encoder + computes PID + writes motor in < 500us
  • Measures tick-to-tick jitter
  • Fails safe (stops motor) if any tick exceeds 900us

Complete Code

#include <horus/horus.hpp>
#include <chrono>
#include <cmath>
#include <cstdio>
#include <algorithm>
using namespace horus::literals;

class RtMotorLoop : public horus::Node {
public:
    RtMotorLoop() : Node("rt_motor_1khz") {
        cmd_sub_  = subscribe<horus::msg::CmdVel>("motor.target");
        enc_sub_  = subscribe<horus::msg::CmdVel>("encoder.rpm");
        pwm_pub_  = advertise<horus::msg::CmdVel>("motor.pwm");
    }

    void init() override {
        last_tick_ = std::chrono::steady_clock::now();
        horus::log::info("rt_motor", "1kHz RT loop initialized");
    }

    void tick() override {
        auto now = std::chrono::steady_clock::now();
        auto dt_us = std::chrono::duration_cast<std::chrono::microseconds>(
            now - last_tick_).count();
        last_tick_ = now;

        // Track jitter statistics
        if (tick_count_ > 10) {  // skip first 10 ticks (startup)
            if (dt_us > max_jitter_us_) max_jitter_us_ = dt_us;
            if (dt_us < min_jitter_us_) min_jitter_us_ = dt_us;
            jitter_sum_ += dt_us;
        }
        tick_count_++;

        // Read target velocity
        if (auto cmd = cmd_sub_->recv()) {
            target_rpm_ = cmd->get()->linear;
        }

        // Read actual RPM
        double actual_rpm = 0;
        if (auto enc = enc_sub_->recv()) {
            actual_rpm = enc->get()->linear;
        }

        // PID (tuned for motor dynamics)
        double error = target_rpm_ - actual_rpm;
        integral_ += error * 0.001;  // dt = 1ms
        integral_ = std::clamp(integral_, -100.0, 100.0);
        double derivative = (error - prev_error_) * 1000.0;
        prev_error_ = error;

        double output = 0.5 * error + 0.01 * integral_ + 0.001 * derivative;
        output = std::clamp(output, -1.0, 1.0);

        horus::msg::CmdVel pwm{};
        pwm.linear = static_cast<float>(output);
        pwm_pub_->send(pwm);

        // Report jitter every 1000 ticks (1 Hz)
        if (tick_count_ % 1000 == 0) {
            double avg = static_cast<double>(jitter_sum_) / (tick_count_ - 10);
            char buf[128];
            std::snprintf(buf, sizeof(buf),
                "jitter: min=%ldus avg=%.0fus max=%ldus (target=1000us)",
                min_jitter_us_, avg, max_jitter_us_);
            horus::log::info("rt_motor", buf);
        }
    }

    void enter_safe_state() override {
        horus::msg::CmdVel stop{};
        pwm_pub_->send(stop);
        integral_ = 0;

        char buf[128];
        std::snprintf(buf, sizeof(buf),
            "SAFE STATE — max jitter was %ldus", max_jitter_us_);
        horus::blackbox::record("rt_motor", buf);
    }

private:
    horus::Subscriber<horus::msg::CmdVel>* cmd_sub_;
    horus::Subscriber<horus::msg::CmdVel>* enc_sub_;
    horus::Publisher<horus::msg::CmdVel>*  pwm_pub_;

    double target_rpm_ = 0;
    double integral_ = 0, prev_error_ = 0;
    int tick_count_ = 0;
    long jitter_sum_ = 0;
    long max_jitter_us_ = 0;
    long min_jitter_us_ = 999999;
    std::chrono::steady_clock::time_point last_tick_;
};

int main() {
    horus::Scheduler sched;
    sched.tick_rate(1000_hz)
         .name("rt_control")
         .prefer_rt();

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

    std::printf("1kHz RT motor loop starting...\n");
    std::printf("Monitor: horus log (see jitter stats)\n");
    sched.spin();
}

Understanding the Timing Budget

|←────────── 1000 us (1 kHz period) ──────────→|
|── budget ──|── slack ──|── deadline ──|
|   500 us   |           |   900 us    |
                                        ^ Miss::SafeMode triggers here
  • budget(500us): expected max computation time
  • deadline(900us): absolute latest the tick can finish
  • slack: 400us buffer for OS scheduling jitter
  • If tick exceeds 900us → enter_safe_state() called → motor stops

Jitter Measurement

The code measures tick-to-tick timing:

  • Ideal: every tick exactly 1000us apart
  • Real (no RT kernel): 950-1200us typical, spikes to 5000us+
  • Real (PREEMPT_RT): 995-1005us, max spike ~50us

Key Takeaways

  • 1 kHz control is achievable on commodity Linux with HORUS
  • pin_core() is critical — prevents OS migration mid-tick
  • budget() + SafeMode = automatic shutdown on timing failure
  • Measure jitter to prove your system meets requirements
  • prefer_rt() for development, require_rt() for production
  • BlackBox records the max jitter at safe-state for post-mortem analysis