Tutorial 10: Write a Reusable Driver (C++)

#include <horus/horus.hpp>
#include <fcntl.h>
#include <unistd.h>
#include <cstring>
#include <cmath>
using namespace horus::literals;

class ImuDriver : public horus::Node {
public:
    ImuDriver(horus::Params& params)
        : Node("imu_driver"), params_(params)
    {
        imu_pub_    = advertise<horus::msg::Imu>("imu.data");
        hb_pub_     = advertise<horus::msg::Heartbeat>("imu.heartbeat");
        status_pub_ = advertise<horus::msg::DiagnosticStatus>("imu.status");
    }

    void init() override {
        const char* port = "/dev/ttyUSB0";  // from params in production
        int baud = static_cast<int>(params_.get<int64_t>("imu.baudrate", 115200));

        fd_ = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
        if (fd_ < 0) {
            horus::log::error("imu", "Failed to open " + std::string(port));
            publish_status(2, "Failed to open serial port");
            return;
        }

        // Configure serial port (abbreviated)
        // ...

        connected_ = true;
        horus::log::info("imu", "IMU connected on " + std::string(port));
        publish_status(0, "Connected and calibrating");

        if (params_.get<bool>("imu.calibrate", true)) {
            calibrate();
        }
    }

    void tick() override {
        tick_count_++;

        if (!connected_) return;

        // Read raw data from IMU (non-blocking)
        uint8_t buf[64];
        int n = fd_ >= 0 ? read(fd_, buf, sizeof(buf)) : -1;

        if (n > 0) {
            parse_and_publish(buf, n);
            consecutive_errors_ = 0;
        } else {
            consecutive_errors_++;
            if (consecutive_errors_ > 100) {  // 1 second at 100 Hz
                horus::log::error("imu", "IMU read timeout — 100 consecutive failures");
                horus::blackbox::record("imu", "Read timeout > 1s");
                publish_status(2, "Read timeout");
                connected_ = false;
            }
        }

        // Heartbeat at 1 Hz
        if (tick_count_ % 100 == 0) {
            horus::msg::Heartbeat hb{};
            std::strncpy(reinterpret_cast<char*>(hb.node_id), "imu_driver", 31);
            hb.sequence = tick_count_ / 100;
            hb.alive = connected_;
            hb_pub_->send(hb);
        }

        // Status at 0.2 Hz
        if (tick_count_ % 500 == 0) {
            publish_status(connected_ ? 0 : 2,
                connected_ ? "OK" : "Disconnected");
        }
    }

    void enter_safe_state() override {
        horus::log::warn("imu", "Safe state — closing device");
        if (fd_ >= 0) { close(fd_); fd_ = -1; }
        connected_ = false;
        horus::blackbox::record("imu", "Safe state entered");
    }

private:
    void calibrate() {
        // Read 100 samples, compute bias
        horus::log::info("imu", "Calibrating (hold still)...");
        // ... calibration logic ...
        horus::log::info("imu", "Calibration complete");
        publish_status(0, "Ready");
    }

    void parse_and_publish(const uint8_t* buf, int len) {
        // Parse device-specific protocol into Imu message
        horus::msg::Imu imu{};
        imu.orientation[3] = 1.0;         // identity quaternion
        imu.angular_velocity[2] = 0.01;   // simulated yaw rate
        imu.linear_acceleration[2] = 9.81; // gravity
        imu.timestamp_ns = 0;

        // Apply calibration offset
        imu.angular_velocity[2] -= gyro_bias_z_;

        imu_pub_->send(imu);
    }

    void publish_status(uint8_t level, const char* msg) {
        horus::msg::DiagnosticStatus status{};
        status.level = level;
        std::strncpy(reinterpret_cast<char*>(status.message), msg, 255);
        status.message_len = std::strlen(msg);
        status_pub_->send(status);
    }

    horus::Params& params_;
    horus::Publisher<horus::msg::Imu>*              imu_pub_;
    horus::Publisher<horus::msg::Heartbeat>*         hb_pub_;
    horus::Publisher<horus::msg::DiagnosticStatus>*  status_pub_;
    int fd_ = -1;
    bool connected_ = false;
    int tick_count_ = 0;
    int consecutive_errors_ = 0;
    double gyro_bias_z_ = 0.0;
};

int main() {
    horus::Params params;
    params.set("imu.baudrate", int64_t(115200));
    params.set("imu.calibrate", true);

    horus::Scheduler sched;
    sched.tick_rate(100_hz).name("imu_node").prefer_rt();

    ImuDriver imu(params);
    sched.add(imu)
        .order(0)
        .budget(2_ms)
        .on_miss(horus::Miss::Warn)
        .watchdog(5_s)
        .build();

    sched.spin();
}

// In another project's main.cpp
ImuDriver imu(params);
sched.add(imu).order(0).budget(2_ms).build();

// Any node can subscribe to imu.data
auto imu_sub = sched.subscribe<horus::msg::Imu>("imu.data");