# Driver High Rate Control

**What you’ll do**
- `Driver`로 **명령 입력** 주기를 $1\,\mathrm{kHz}$까지 올리는 설정을 확인합니다.
- **속도 요청** 주기를 조절하며 통신 여유/성능을 점검합니다.

**Prerequisites**
- {doc}`Driver Demo </page/SDK/tutorials/driver_demo>`

**Next**
- {doc}`Driver Tutorials </page/SDK/tutorials/driver_tutorials>`
- {doc}`SDK Guide </page/SDK/sdk_guide>`

---

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

```{admonition} 주의
:class: warning

$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) 고속 통신 설정 (**명령 입력**/**속도 요청**)
옵션 필드 의미는 {ref}`Driver::Options <sdk-driver-options>`를 기준으로 합니다.

```cpp
  // 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)`를 주기적으로 업데이트합니다.

```cpp
  // 상위 목표값 업데이트: 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`으로 하한 적용

```cpp
  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 등에서 지터를 더 줄이고 싶다면 사용합니다.

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

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

```cpp
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`와 동일한 내용입니다.

```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;
}
```