Skip to content

Custom Input Driver

Jump to bottom
konodoki edited this page May 13, 2026 · 7 revisions

手把手添加自定义遥控器驱动

本页说明如何接入新的遥控器或协议，例如 CRSF、SBUS、串口、UDP、自定义 HID。

核心原则：

driver 只生产 raw source
YAML 解释 raw source 的业务含义
业务状态只看状态机 event

driver 不应该知道 normal、back_flip、btn_10=1 这些业务名字。

1. 什么时候不需要写 driver

只改 YAML 就够的情况：

  • 换一个 Linux joystick 手柄，只是轴号和按钮号不同。
  • 改键盘按键。
  • 改组合键。
  • 把扳机当按钮，用阈值判断。
  • 把三档开关当 enum，用区间判断。

这类需求看 遥控器 YAML 配置。

2. 什么时候需要写 driver

需要新增 InputDriver 的情况：

  • 输入设备不是 Linux joystick。
  • 协议需要自己解析，例如 CRSF / SBUS。
  • 输入来自串口、UDP、TCP。
  • 需要读自定义 HID report。
  • 设备连接状态需要自定义判断。

3. 先设计 raw source

先不要写代码，先定 raw source 名。

CRSF 示例：

crsf.ch1
crsf.ch2
crsf.ch3
crsf.ch4
crsf.ch5
crsf.link

命名建议：

  • 用协议或设备名前缀，例如 crsf.、sbus.、udp.。
  • 通道保持中性名字，例如 ch1、ch2。
  • 归一化后的轴值统一为 [-1, 1]。
  • 按钮值统一为 0.0 / 1.0。
  • 三档开关也可以作为连续值输出，再由 YAML 的 enum 解释。

4. 在 YAML 声明 source

sources:
  crsf:
    type: crsf
    signals:
      crsf.roll: {from: crsf.ch1, timeout_ms: 200, failsafe: 0.0}
      crsf.pitch: {from: crsf.ch2, timeout_ms: 200, failsafe: 0.0}
      crsf.throttle: {from: crsf.ch3, timeout_ms: 200, failsafe: 0.0}
      crsf.yaw: {from: crsf.ch4, timeout_ms: 200, failsafe: 0.0}
      crsf.mode: {from: crsf.ch5, timeout_ms: 200, failsafe: 0.0}

然后 controls：

controls:
  move.vx:
    type: analog
    source: crsf.pitch
    direction: -1
    curve: stick
    alpha: 0.03

  switch.mode:
    type: enum
    source: crsf.mode
    default: middle
    hysteresis: 0.03
    positions:
      low: [-1.0, -0.35]
      middle: [-0.34, 0.34]
      high: [0.35, 1.0]

输出：

outputs:
  edge:
    - output: btn_10=5
      when: [switch.mode=high]

这说明：driver 不需要知道 btn_10=5。它只写 crsf.ch5。

5. 如果 driver 需要配置参数

例如 CRSF 需要串口路径和波特率：

sources:
  crsf:
    type: crsf
    device: /dev/ttyUSB0
    baudrate: 420000
    signals:
      crsf.roll: {from: crsf.ch1, timeout_ms: 200, failsafe: 0.0}

需要在 C++ config 增加字段。

config.hpp：

struct CrsfConfig {
    std::string device = "/dev/ttyUSB0";
    int baudrate = 420000;
};

struct RemoteConfig {
    // ...
    CrsfConfig crsf;
};

config.cpp 的 load_sources() 中：

if (type == "crsf") {
    config.crsf.device = get_or<std::string>(
        group_node,
        "device",
        config.crsf.device);
    config.crsf.baudrate = get_or<int>(
        group_node,
        "baudrate",
        config.crsf.baudrate);
}

如果 driver 不需要任何专用参数，只用 signals 就够。

6. 文件结构

大型 driver 不要继续塞进 input_driver.cpp。

推荐新增：

src/remote_controller/include/remote_controller/crsf_input_driver.hpp
src/remote_controller/src/crsf_input_driver.cpp

头文件：

#pragma once

#include <memory>
#include <mutex>

#include "remote_controller/input_driver.hpp"

namespace remote_controller {

std::unique_ptr<InputDriver> create_crsf_input_driver(
    InputMapper &mapper,
    std::mutex &mapper_lock,
    DriverOutputHandler output_handler,
    DriverLogHandler log_handler);

}  // namespace remote_controller

7. driver 基本骨架

crsf_input_driver.cpp：

#include "remote_controller/crsf_input_driver.hpp"

#include <atomic>
#include <chrono>
#include <mutex>
#include <string>
#include <thread>
#include <utility>
#include <vector>

namespace remote_controller {
namespace {

class CrsfInputDriver : public InputDriver {
public:
    CrsfInputDriver(
        InputMapper &mapper,
        std::mutex &mapper_lock,
        DriverOutputHandler output_handler,
        DriverLogHandler log_handler)
        : mapper_(mapper),
          mapper_lock_(mapper_lock),
          output_handler_(std::move(output_handler)),
          log_handler_(std::move(log_handler))
    {
    }

    ~CrsfInputDriver() override
    {
        stop();
    }

    std::string name() const override
    {
        return "crsf";
    }

    void start() override
    {
        stop_flag_ = false;
        thread_ = std::thread(&CrsfInputDriver::run, this);
    }

    void stop() override
    {
        stop_flag_ = true;
        close_device();
        if (thread_.joinable()) {
            thread_.join();
        }
    }

private:
    InputMapper &mapper_;
    std::mutex &mapper_lock_;
    DriverOutputHandler output_handler_;
    DriverLogHandler log_handler_;
    std::atomic<bool> stop_flag_{false};
    std::thread thread_;

    void run()
    {
        while (!stop_flag_) {
            if (!ensure_device_open()) {
                sleep_short();
                continue;
            }

            if (!wait_frame_readable()) {
                touch_runtime_sources();
                continue;
            }

            CrsfFrame frame;
            if (!read_frame(frame)) {
                continue;
            }

            std::vector<std::string> outputs;
            {
                const std::lock_guard<std::mutex> guard(mapper_lock_);
                append(outputs, mapper_.set_signal("crsf.ch1", normalize(frame.ch[0])));
                append(outputs, mapper_.set_signal("crsf.ch2", normalize(frame.ch[1])));
                append(outputs, mapper_.set_signal("crsf.ch3", normalize(frame.ch[2])));
                append(outputs, mapper_.set_signal("crsf.ch4", normalize(frame.ch[3])));
                append(outputs, mapper_.set_signal("crsf.ch5", normalize(frame.ch[4])));
                mapper_.touch_runtime_sources_with_prefix("crsf.");
            }
            dispatch(outputs);
        }
    }

    void append(std::vector<std::string> &dst, const std::vector<std::string> &src)
    {
        dst.insert(dst.end(), src.begin(), src.end());
    }

    double normalize(int raw) const
    {
        // 按真实协议范围映射到 [-1, 1]。
        return 0.0;
    }

    void touch_runtime_sources()
    {
        const std::lock_guard<std::mutex> guard(mapper_lock_);
        mapper_.touch_runtime_sources_with_prefix("crsf.");
    }

    void dispatch(const std::vector<std::string> &outputs) const
    {
        if (!outputs.empty() && output_handler_) {
            output_handler_(outputs);
        }
    }

    void log(const std::string &message) const
    {
        if (log_handler_) {
            log_handler_(message);
        }
    }
};

}  // namespace

std::unique_ptr<InputDriver> create_crsf_input_driver(
    InputMapper &mapper,
    std::mutex &mapper_lock,
    DriverOutputHandler output_handler,
    DriverLogHandler log_handler)
{
    return std::unique_ptr<InputDriver>(new CrsfInputDriver(
        mapper,
        mapper_lock,
        std::move(output_handler),
        std::move(log_handler)));
}

}  // namespace remote_controller

上面是结构骨架，以下函数需要按真实协议实现：

ensure_device_open()
wait_frame_readable()
read_frame()
close_device()
sleep_short()
CrsfFrame

8. 接入 factory

打开：

src/remote_controller/src/input_driver.cpp

加入 include：

#include "remote_controller/crsf_input_driver.hpp"

在 create_input_driver() 中添加：

if (driver_type == "crsf") {
    return create_crsf_input_driver(
        mapper,
        mapper_lock,
        std::move(output_handler),
        std::move(log_handler));
}

9. 修改 CMakeLists

打开：

src/remote_controller/CMakeLists.txt

把新源文件加入 core library：

add_library(${PROJECT_NAME}_core
  src/config.cpp
  src/input_driver.cpp
  src/input_mapper.cpp
  src/motion_commands_adapter.cpp
  src/crsf_input_driver.cpp
)

10. 启动新 driver

命令行：

ros2 run remote_controller remote_controller --driver crsf --config src/remote_controller/config/xbox_default.yaml

launch：

arguments=["--driver", "crsf", "--config", remote_config, "__log_level:=debug"]

11. 阻塞读取和 Ctrl+C

driver 主循环不能永久阻塞在 read()。否则 Ctrl+C 后线程无法退出，必须等设备再来一次输入。

推荐：

  • fd 用非阻塞模式。
  • 用 select() 或 poll() 带超时等待。
  • 每次超时检查 stop_flag_。
  • stop() 里关闭 fd，打断阻塞。

伪代码：

while (!stop_flag_) {
    int ready = poll(&pfd, 1, 100);
    if (stop_flag_) {
        break;
    }
    if (ready == 0) {
        touch_runtime_sources();
        continue;
    }
    if (ready < 0) {
        reconnect();
        continue;
    }
    read_frame();
}

12. failsafe 和心跳

timeout_ms 的语义是：设备或协议帧断联一段时间后触发，不是“输入值没变化一段时间后触发”。

所以 driver 必须区分：

设备连接正常，只是值没变
  -> touch_runtime_sources_with_prefix("crsf.")

设备断开，或者长期没有有效帧
  -> 不 touch，让 timeout_ms/failsafe 生效

如果不 touch，摇杆保持不动超过 timeout 就会被误判断联。

13. 多通道同步问题

当前 set_signal() 每次写一个 raw source 都会刷新 controls 和 bindings。第一版通常够用。

如果协议帧里同时包含多个通道，而组合键需要严格基于同一帧判断，可以后续给 InputMapper 增加批量接口：

std::vector<std::string> set_signals(
    const std::map<std::string, double> &signals);

一次写完所有通道，再刷新一次 binding。

14. driver checklist

1. 设计 raw source 名，例如 crsf.ch1。
2. 在 YAML sources 声明 semantic source -> raw source。
3. 在 controls/outputs 中复用业务控制逻辑。
4. 新增 driver 头文件和 cpp。
5. 实现 InputDriver::name/start/stop。
6. run() 中读取协议帧并 mapper_.set_signal()。
7. 连接正常但值不变时 touch runtime sources。
8. 断联时不要 touch，让 failsafe 生效。
9. create_input_driver() 注册 driver_type。
10. CMakeLists.txt 加源文件。
11. colcon build。
12. ros2 topic echo /motion_commands 验证输出。

快速开始

Mod 与状态开发

模型、推理与真机

输入系统

平台接入与控制时序

配置与 API 参考

框架架构与维护

Clone this wiki locally