Driver High Rate Control

Driver High Rate Control#

What you’ll do

  • Driver로 명령 입력 주기를 \(1\,\mathrm{kHz}\)까지 올리는 설정을 확인합니다.

  • 속도 요청 주기를 조절하며 통신 여유/성능을 점검합니다.

Prerequisites

Next

이 예제는 Jetson 환경에서 \(1\,\mathrm{kHz}\) 명령 입력 주기를 유지하기 위한 옵션/스케줄링/출력 설정을 다룹니다.

주의

\(1\,\mathrm{kHz}\) 제어는 baud rate, RTS/CTS, 시스템 부하에 따라 실패할 수 있습니다. 우선 낮은 주기/낮은 속도에서 동작을 확인한 뒤 올리세요.

1. 목표#

  • 명령 입력(주행 제어) 송신을 \(1\,\mathrm{kHz}\)로 안정적으로 유지

  • 속도 요청(차량 속도)을 가능한 범위까지 끌어올려 통신 여유/성능 확인

  • 실제 시스템처럼 setCommand(v, omega)를 주기적으로 업데이트(예: \(50\,\mathrm{Hz}\))

2. 설계 원칙#

고속 제어 시스템을 설계할 때는 **”생성 주기”**와 **”전송 주기”**를 명확히 구분해야 합니다.

  • Host Algorithm (User Thread): 인지/판단 알고리즘은 연산 비용이 높으므로 상대적으로 느린 주기로 목표값(\(v, \omega\))을 갱신합니다.

  • Driver I/O (Internal Thread): 하드웨어 제어기는 부드러운 구동을 위해 매우 빠른 주기(ex: \(1\,\mathrm{kHz}\))로 최신 명령을 수신해야 합니다. Driver는 이 역할을 수행하며, 사용자 스레드와 비동기적으로 동작합니다.

###권장 하드웨어 설정

  • Baudrate: \(921{,}600\,\mathrm{bps}\) 또는 \(1{,}000{,}000\,\mathrm{bps}\)

  • OS Setting: realtime_priority 설정을 통한 스레드 스케줄링 우선순위 상향 (Root 권한 필요 가능성 있음)

준비물(권장)#

  • baud: 921600 또는 1000000

  • Linux에서 지터를 더 줄이려면 realtime_priority 사용(권한 필요할 수 있음)

3. 입력 파라미터#

  • port: 디바이스 경로 (예: /dev/ttyKMC)

  • seconds: 실행 시간 (기본 5)

  • baud: UART 보드레이트 (기본 1000000)

  • vehicle_speed_hz: 속도 요청 주기 (기본 100)

  • command_hz: setCommand(v, omega) 업데이트 주기 (기본 50)

4. 단계별 구현 가이드#

단계 1) 고속 통신 설정 (명령 입력/속도 요청)#

옵션 필드 의미는 Driver::Options를 기준으로 합니다.

  // 1. Baudrate 상향
  opt.serial.baudrate = 921600; // 또는 1000000

  // 2. 명령 입력(제어) 전송 1 kHz
  opt.control_rate_hz = 1000.0;

  // 3. 속도 요청도 가능한 만큼 상향
  opt.vehicle_speed_rate_hz = 1000.0; // 보드/펌웨어가 버티면 1 kHz

단계 2) setCommand(v, omega) 업데이트 주기 설정#

command_hz에 맞춰 setCommand(v, omega)를 주기적으로 업데이트합니다.

  // 상위 목표값 업데이트: 50 Hz
  const double cmd_hz = 50.0;
  // ...
  if (now >= next_cmd) {
    drv.setCommand(1.0f, 0.0f);
    next_cmd += cmd_period;
  }

단계 3) command_timeout_ms 계산#

사용자 알고리즘(Main Thread)이 멈췄을 때 차량을 세우기 위한 Command Timeout은 사용자 업데이트 주기를 기준으로 계산해야 합니다. Driver의 송신 주기(\(1\,\mathrm{kHz}\))가 기준이 아님에 유의하십시오. 권장 :

  • command_timeout_ms = 2 * (1000 / command_hz)

  • 최소값 50으로 하한 적용

  if (cmd_hz > 0.0) {
    const double period_ms = 1000.0 / cmd_hz;
    opt.command_timeout_ms = static_cast<int>(std::max(50.0, period_ms * 2.0));
  }

단계 4) Linux Thread Hint 적용 (Advanced)#

Jetson 등에서 지터를 더 줄이고 싶다면 사용합니다.

  // Linux only: SCHED_FIFO 우선순위 (1~99, 높을수록 우선순위 높음)
  opt.realtime_priority = 70; // SCHED_FIFO (권한 필요할 수 있음)
  opt.cpu_affinity = -1;      // -1: 비활성, 0~N: 특정 CPU 코어 고정

단계 5) 루프 구조와 출력 해석#

if (cmd_hz > 0.0 && now >= next_cmd) {
  drv.setCommand(1.0f, 0.0f);
  ++cmd_updates;
  next_cmd += cmd_period;
  if (next_cmd <= now) next_cmd = now + cmd_period;
}

if (now >= next_status) {
  std::printf("[1kHz] t=%llds cmd_updates=%zu\n", ...);
  next_status += std::chrono::seconds(1);
}

  • cmd_updates는 1초 동안의 명령 업데이트 횟수입니다.

  • next_cmd <= now 체크로 지연 누적을 방지합니다.

5. Result#

아래 코드는 SDK 리포지토리의 examples/Driver_Intermediate/high_rate_control.cpp와 동일한 내용입니다.

#include "KMC_driver.hpp"

#include <algorithm>
#include <chrono>
#include <cstdio>
#include <cstdlib>
#include <string>
#include <thread>

int main(int argc, char **argv) {
  const std::string port = (argc > 1) ? argv[1] : "/dev/ttyKMC";
  int seconds = (argc > 2) ? std::atoi(argv[2]) : 5;
  if (seconds <= 0) seconds = 5;
  const int baud = (argc > 3) ? std::atoi(argv[3]) : 1000000;
  const double b3_rate = (argc > 4) ? std::atof(argv[4]) : 100.0;
  const double cmd_hz = (argc > 5) ? std::atof(argv[5]) : 50.0;

  KMC_HARDWARE::Driver::Options opt;
  opt.port = port;
  opt.serial.baudrate = baud > 0 ? baud : 1000000;
  opt.serial.hw_flow_control = true;

  opt.control_rate_hz = 1000.0;
  opt.vehicle_speed_rate_hz = b3_rate > 0 ? b3_rate : 100.0;

  if (cmd_hz > 0.0) {
    const double period_ms = 1000.0 / cmd_hz;
    opt.command_timeout_ms = static_cast<int>(std::max(50.0, period_ms * 2.0));
  } else {
    opt.command_timeout_ms = 0;
  }

  opt.stop_burst_count = 5;

  opt.realtime_priority = 70;
  opt.cpu_affinity = -1;

  KMC_HARDWARE::Driver drv;
  if (!drv.start(opt)) {
    std::fprintf(stderr, "Failed to open %s\n", port.c_str());
    return 2;
  }

  drv.setCommand(1.0f, 0.0f);

  const auto t0 = std::chrono::steady_clock::now();
  auto next_status = t0;
  auto next_cmd = t0;
  const auto cmd_period =
      (cmd_hz > 0.0)
          ? std::chrono::duration_cast<std::chrono::steady_clock::duration>(
                std::chrono::duration<double>(1.0 / cmd_hz))
          : std::chrono::steady_clock::duration::zero();
  size_t cmd_updates = 0;

  while (std::chrono::steady_clock::now() - t0 <
         std::chrono::seconds(seconds)) {
    const auto now = std::chrono::steady_clock::now();

    if (cmd_hz > 0.0 && now >= next_cmd) {
      drv.setCommand(1.0f, 0.0f);
      ++cmd_updates;
      next_cmd += cmd_period;
      if (next_cmd <= now) {
        next_cmd = now + cmd_period;
      }
    }

    if (now >= next_status) {
      const auto elapsed =
          std::chrono::duration_cast<std::chrono::seconds>(now - t0).count();
      std::printf("[1kHz] t=%llds cmd_updates=%zu\n",
                  static_cast<long long>(elapsed), cmd_updates);
      next_status += std::chrono::seconds(1);
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(1));
  }

  drv.setCommand(0.0f, 0.0f);
  drv.stop();
  return 0;
}
Contents