Add YFR4 ESC driver

Author: Apehaenger

CMakeLists.txt

@@ -76,6 +76,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
         src/drivers/motor/vesc/buffer.cpp
         src/drivers/motor/vesc/crc.cpp
         src/drivers/motor/vesc/VescDriver.cpp
+        # YFR4-ESC driver
+        src/drivers/motor/yfr4esc/YFR4escDriver.cpp
         # PWM motor driver
         src/drivers/motor/pwm/pwm_motor_driver.cpp
         # GPS driver

robots/CMakeLists.txt

@@ -8,3 +8,6 @@ endif ()
 
 # find the platform sources and add them
 file(GLOB_RECURSE PLATFORM_SOURCES "${CMAKE_CURRENT_LIST_DIR}/src/*")
+
+# Expose a preprocessor guard like ROBOT_PLATFORM_YardForce_V4=1 for #ifdef use
+add_compile_definitions(ROBOT_PLATFORM_${ROBOT_PLATFORM}=1)

robots/include/robot.hpp

@@ -2,6 +2,7 @@
 #define ROBOT_HPP
 
 #include <drivers/motor/vesc/VescDriver.h>
+#include <drivers/motor/yfr4esc/YFR4escDriver.h>
 #include <hal.h>
 
 #include <debug/debug_tcp_interface.hpp>
@@ -48,7 +49,14 @@ class MowerRobot : public Robot {
 
   xbot::driver::motor::VescDriver left_motor_driver_{};
   xbot::driver::motor::VescDriver right_motor_driver_{};
-  xbot::driver::motor::VescDriver mower_motor_driver_{};
+
+  // Select mower motor ESC by platform: YFR4 on YardForce_V4, VESC otherwise
+#if defined(ROBOT_PLATFORM_YardForce_V4)
+  using MowerEscDriver = xbot::driver::motor::YFR4escDriver;
+#else
+  using MowerEscDriver = xbot::driver::motor::VescDriver;
+#endif
+  MowerEscDriver mower_motor_driver_{};
 
   DebugTCPInterface left_esc_driver_interface_{65102, &left_motor_driver_};
   DebugTCPInterface mower_esc_driver_interface_{65103, &mower_motor_driver_};

robots/src/yardforce_v4_robot.cpp

@@ -3,6 +3,7 @@
 #include <services.hpp>
 
 void YardForce_V4Robot::InitPlatform() {
+  // Initialize default motors (VESC for left/right + mower)
   InitMotors();
   charger_.setI2C(&I2CD1);
   power_service.SetDriver(&charger_);

src/drivers/motor/yfr4esc/YFR4escDriver.cpp

@@ -0,0 +1,249 @@
+//
+// Created by GitHub Copilot on 16.08.25.
+//
+
+#include "YFR4escDriver.h"
+
+#include <cstdio>
+#include <cstring>
+
+#include "cobs.h"
+#include "crc16_hw.hpp"
+#define LOG_TAG_STR "YFR4esc"
+#include "ulog_rate_limit.hpp"
+
+namespace xbot::driver::motor {
+
+using namespace yfr4esc;
+
+// Event when the RX DMA buffer wrapped (full) and was re-armed.
+static constexpr uint32_t EVT_RX_DMA_WRAP = 1;
+
+bool YFR4escDriver::SetUART(UARTDriver *uart, uint32_t baudrate) {
+  chDbgAssert(!IsStarted(), "Only set UART when the driver is stopped");
+  chDbgAssert(uart != nullptr, "need to provide a driver");
+  if (IsStarted()) return false;
+
+  uart_ = uart;
+  uart_config_.speed = baudrate;
+  uart_config_.context = this;
+
+  return true;
+}
+
+void YFR4escDriver::ProcessDecodedPacket(uint8_t *packet, size_t len) {
+  // Packages have to be at least 1 byte of type + 1 byte of data + 2 bytes of CRC
+  if (len < 4) {
+    ULOGT_EVERY_MS(WARNING, 500, "Decoded packet too short (%zu bytes). Dropping!", len);
+    return;
+  }
+
+  // Check CRC (on-wire is little-endian; STM32 is little-endian too)
+  uint16_t crc_rx;
+  memcpy(&crc_rx, &packet[len - 2], sizeof(crc_rx));  // memcpy avoids potential unaligned access
+  uint16_t crc_calc = yfr4esc_crc::crc16_ccitt_false(packet, len - 2);
+  if (crc_rx != crc_calc) {
+    ULOGT_EVERY_MS(WARNING, 500, "CRC mismatch (rx=0x%04X calc=0x%04X len=%u). Dropping!", crc_rx, crc_calc,
+                   (unsigned)len);
+    return;
+  }
+
+  uint8_t msg_type = packet[0];
+  switch (msg_type) {
+    case MessageType::STATUS: {
+      if (len != sizeof(StatusPacket)) {
+        ULOGT_EVERY_MS(WARNING, 500, "Status packet length mismatch (%u != %zu). Dropping!", (unsigned)len,
+                       sizeof(StatusPacket));
+        break;
+      }
+      StatusPacket sp{};
+      memcpy(&sp, packet, sizeof(StatusPacket));
+
+      latest_state_.fw_major = sp.fw_version_major;
+      latest_state_.fw_minor = sp.fw_version_minor;
+      latest_state_.temperature_pcb = static_cast<float>(sp.temperature_pcb);
+      latest_state_.current_input = static_cast<float>(sp.current_input);
+      latest_state_.duty_cycle = static_cast<float>(sp.duty_cycle);
+      latest_state_.direction = sp.direction ? 1.0f : 0.0f;
+      latest_state_.tacho = sp.tacho;
+      latest_state_.tacho_absolute = sp.tacho_absolute;
+      latest_state_.rpm = static_cast<float>(sp.rpm);
+      latest_state_.status =
+          sp.fault_code == 0 ? ESCState::ESCStatus::ESC_STATUS_OK : ESCState::ESCStatus::ESC_STATUS_ERROR;
+
+      if (sp.fault_code & (0b1 << FaultBit::UNINITIALIZED)) {
+        SendSettings();
+      }
+
+      /* Debugging
+      ULOGT_EVERY_MS(
+          INFO, 1000, "Status: fw=%u.%u temp=%.2fC I_in=%.3fA duty=%.3f dir=%u tacho=%u tacho_abs=%u rpm=%u fault=%d",
+          latest_state_.fw_major, latest_state_.fw_minor, latest_state_.temperature_pcb, latest_state_.current_input,
+          latest_state_.duty_cycle, (unsigned)latest_state_.direction, latest_state_.tacho,
+          latest_state_.tacho_absolute, (unsigned)latest_state_.rpm, (int)sp.fault_code); */
+
+      NotifyCallback();
+      break;
+    }
+    default: ULOGT_EVERY_MS(WARNING, 500, "Unknown msg type: 0x%02X len=%u. Dropping!", msg_type, (unsigned)len); break;
+  }
+}
+
+void YFR4escDriver::ProcessRxBytes(const volatile uint8_t *data, size_t len) {
+  if (len == 0) return;
+  if (IsRawMode()) {
+    RawDataOutput(const_cast<uint8_t *>(data), len);
+    return;
+  }
+  for (size_t i = 0; i < len; ++i) {
+    uint8_t b = data[i];
+    if (b == 0) {  // COBS end marker
+      if (cobs_rx_len_ > 0) {
+        size_t dec = cobs_decode(cobs_rx_buffer_, cobs_rx_len_, cobs_decoded_);
+        if (dec >= 3) {
+          ProcessDecodedPacket(cobs_decoded_, dec);
+        }
+        cobs_rx_len_ = 0;
+      }
+    } else {
+      if (cobs_rx_len_ < COBS_BUFFER_SIZE) {
+        cobs_rx_buffer_[cobs_rx_len_++] = b;
+      } else {
+        cobs_rx_len_ = 0;  // overflow, reset
+      }
+    }
+  }
+}
+
+void YFR4escDriver::SetDuty(float duty) {
+  if (!IsStarted() || IsRawMode()) return;
+  last_duty_ = duty;
+  SendControl(duty);
+}
+
+void YFR4escDriver::SendControl(float duty) {
+  ControlPacket cp{.message_type = MessageType::CONTROL, .duty_cycle = static_cast<double>(duty), .crc = 0};
+  const uint8_t *payload = reinterpret_cast<const uint8_t *>(&cp);
+  cp.crc = yfr4esc_crc::crc16_ccitt_false(payload, sizeof(ControlPacket) - sizeof(cp.crc));
+
+  chMtxLock(&mutex_);  // protect shared tx_buffer_ and send
+  size_t len = cobs_encode(reinterpret_cast<const uint8_t *>(&cp), sizeof(cp), tx_buffer_);
+  if (len + 1 > TX_BUFFER_SIZE) {
+    ULOGT_EVERY_MS(WARNING, 1000, "TX control frame too large: len=%u", (unsigned)len);
+    chMtxUnlock(&mutex_);
+    return;
+  }
+  tx_buffer_[len++] = 0;  // COBS end marker
+  uartSendFullTimeout(uart_, &len, tx_buffer_, TIME_INFINITE);
+  chMtxUnlock(&mutex_);
+}
+
+void YFR4escDriver::SendSettings() {
+  // Defaults. How get those set?
+  SettingsPacket sp{.message_type = MessageType::SETTINGS,
+                    .motor_current_limit = 2.5f,
+                    .min_pcb_temp = 0.0f,
+                    .max_pcb_temp = 70.0f,
+                    .crc = 0};
+
+  // Compute CRC over message_type + data only (exclude crc), store as-is (LE on wire)
+  const uint8_t *payload = reinterpret_cast<const uint8_t *>(&sp);
+  sp.crc = yfr4esc_crc::crc16_ccitt_false(payload, sizeof(SettingsPacket) - sizeof(sp.crc));
+
+  chMtxLock(&mutex_);  // protect shared tx_buffer_ and send
+  size_t len = cobs_encode(reinterpret_cast<const uint8_t *>(&sp), sizeof(SettingsPacket), tx_buffer_);
+  if (len + 1 > TX_BUFFER_SIZE) {
+    ULOGT_EVERY_MS(WARNING, 1000, "TX settings frame too large: len=%u", (unsigned)len);
+    chMtxUnlock(&mutex_);
+    return;
+  }
+  tx_buffer_[len++] = 0;  // COBS end marker
+  uartSendFullTimeout(uart_, &len, tx_buffer_, TIME_INFINITE);
+  chMtxUnlock(&mutex_);
+}
+
+void YFR4escDriver::RawDataInput(uint8_t *data, size_t size) {
+  if (!IsRawMode() || !IsStarted()) return;
+  chMtxLock(&mutex_);
+  size_t len = size > TX_BUFFER_SIZE ? TX_BUFFER_SIZE : size;
+  memcpy(tx_buffer_, data, len);
+  uartSendFullTimeout(uart_, &len, tx_buffer_, TIME_INFINITE);
+  chMtxUnlock(&mutex_);
+}
+
+bool YFR4escDriver::Start() {
+  chDbgAssert(!IsStarted(), "don't start the driver twice");
+  if (IsStarted()) return false;
+
+  // Configure RX callback
+  uart_config_.rxend_cb = [](UARTDriver *uartp) {
+    chSysLockFromISR();
+    YFR4escDriver *instance = reinterpret_cast<const UARTConfigEx *>(uartp->config)->context;
+    chDbgAssert(instance != nullptr, "instance cannot be null!");
+    // Buffer reached full length. Re-arm DMA immediately to minimize RX gap, then signal the thread.
+    uartStartReceiveI(uartp, DMA_RX_BUFFER_SIZE, const_cast<uint8_t *>(instance->dma_rx_buffer_));
+    // Tell the thread a wrap occurred; it will handle the final tail and reset rx_seen_len_.
+    if (instance->processing_thread_) {
+      chEvtSignalI(instance->processing_thread_, EVT_RX_DMA_WRAP);
+    }
+    chSysUnlockFromISR();
+  };
+
+  if (!(uartStart(uart_, &uart_config_) == MSG_OK)) return false;
+
+  // Set the started flag before launching the thread to avoid race where IsStarted() is false in threadFunc
+  if (!MotorDriver::Start()) return false;
+
+  // Now start the processing thread; IsStarted() should be true now and the thread will spin
+  processing_thread_ = chThdCreateStatic(&thd_wa_, sizeof(thd_wa_), NORMALPRIO, threadHelper, this);
+#ifdef USE_SEGGER_SYSTEMVIEW
+  processing_thread_->name = "YFR4escDriver";
+#endif
+
+  // Arm RX
+  rx_seen_len_ = 0;
+  uartStartReceive(uart_, DMA_RX_BUFFER_SIZE, const_cast<uint8_t *>(dma_rx_buffer_));
+
+  // Send an initial settings packet
+  SendSettings();
+
+  return MotorDriver::IsStarted();
+}
+
+void YFR4escDriver::threadFunc() {
+  systime_t last_heartbeat = 0;
+
+  while (IsStarted()) {
+    uint32_t events;
+
+    // YFRev4-ESC always send a status packet every 20ms.
+    // So, read with timeout, instead of waiting for a buffer full ISR call.
+    events = chEvtWaitOneTimeout(EVT_RX_DMA_WRAP, TIME_MS2I(10));
+    (void)events;
+
+    // Process newly arrived bytes immediately using NDTR deltas; handle wrap if the buffer was refilled.
+    uint32_t ndtr_now = uart_->dmarx->stream->NDTR;
+    if (ndtr_now <= DMA_RX_BUFFER_SIZE) {
+      size_t received_so_far = DMA_RX_BUFFER_SIZE - ndtr_now;
+      if (received_so_far < rx_seen_len_) {
+        // Buffer wrapped (rxend_cb re-armed). Process tail [rx_seen_len_ .. end), then reset.
+        ProcessRxBytes(dma_rx_buffer_ + rx_seen_len_, DMA_RX_BUFFER_SIZE - rx_seen_len_);
+        rx_seen_len_ = 0;
+      }
+      if (received_so_far > rx_seen_len_) {
+        size_t delta = received_so_far - rx_seen_len_;
+        ProcessRxBytes(dma_rx_buffer_ + rx_seen_len_, delta);
+        rx_seen_len_ = received_so_far;
+      }
+    }
+
+    // Heartbeat: send control periodically even if unchanged, to satisfy ESC watchdog
+    systime_t now = chVTGetSystemTimeX();
+    if ((systime_t)(now - last_heartbeat) >= HEARTBEAT_INTERVAL) {
+      last_heartbeat = now;
+      SendControl(last_duty_);
+    }
+  }
+}
+
+}  // namespace xbot::driver::motor

src/drivers/motor/yfr4esc/YFR4escDriver.h

@@ -0,0 +1,87 @@
+//
+// Created by Apehaenger <joerg@ebeling.ws> on 16.08.25.
+//
+
+#ifndef YFR4ESCDRIVER_H
+#define YFR4ESCDRIVER_H
+
+#include <cstdint>
+#include <debug/debuggable_driver.hpp>
+#include <drivers/motor/motor_driver.hpp>
+
+#include "ch.h"
+#include "datatypes.h"
+#include "hal.h"
+
+namespace xbot::driver::motor {
+class YFR4escDriver : public DebuggableDriver, public MotorDriver {
+ public:
+  YFR4escDriver() {
+    latest_state_.status = ESCState::ESCStatus::ESC_STATUS_DISCONNECTED;
+  };
+  ~YFR4escDriver() override = default;
+
+  bool SetUART(UARTDriver *uart, uint32_t baudrate);
+
+  void RequestStatus() override{};  // No-op, ESC streams status periodically
+  void SetDuty(float duty) override;
+  bool Start() override;
+
+  void RawDataInput(uint8_t *data, size_t size) override;
+
+ private:
+  // Heartbeat: resend control regularly to satisfy ESC watchdog
+  static constexpr systime_t HEARTBEAT_INTERVAL = TIME_MS2I(100);  // 10 Hz
+
+  float last_duty_ = 0.0f;
+
+  struct UARTConfigEx : UARTConfig {
+    YFR4escDriver *context;
+  };
+
+  // RX buffer size calc: We only receive Status packets
+  static constexpr size_t RX_MAX_PKT_DECODED = sizeof(yfr4esc::StatusPacket);
+  static constexpr size_t RX_MAX_PKT_CODED = RX_MAX_PKT_DECODED + (RX_MAX_PKT_DECODED / 254) + 1;  // COBS worst-case
+
+  // RX DMA buffer holds two full encoded frames + delimiter for wrap/delta comfort
+  static constexpr size_t DMA_RX_BUFFER_SIZE = 2 * (RX_MAX_PKT_CODED + 1);
+  volatile uint8_t dma_rx_buffer_[DMA_RX_BUFFER_SIZE]{};
+
+  // COBS RX buffer (enough for one full encoded Status frame (without trailing 0))
+  static constexpr size_t COBS_BUFFER_SIZE = RX_MAX_PKT_CODED;
+  uint8_t cobs_rx_buffer_[COBS_BUFFER_SIZE]{};
+  // Decoded buffer holds one Status frame
+  uint8_t cobs_decoded_[RX_MAX_PKT_DECODED]{};
+
+  // DMA-safe TX buffer (avoid sending from stack/DTCM)
+  static constexpr size_t TX_MAX_PKT_DECODED = (sizeof(yfr4esc::ControlPacket) > sizeof(yfr4esc::SettingsPacket))
+                                                   ? sizeof(yfr4esc::ControlPacket)
+                                                   : sizeof(yfr4esc::SettingsPacket);
+  static constexpr size_t TX_MAX_PKT_CODED = TX_MAX_PKT_DECODED + (TX_MAX_PKT_DECODED / 254) + 1;  // COBS worst-case
+  // One full encoded TX frame including trailing 0
+  static constexpr size_t TX_BUFFER_SIZE = TX_MAX_PKT_CODED + 1;
+  uint8_t tx_buffer_[TX_BUFFER_SIZE]{};
+
+  size_t cobs_rx_len_ = 0;
+  size_t rx_seen_len_ = 0;  // Track how many bytes we already processed in the receiving DMA buffer
+
+  THD_WORKING_AREA(thd_wa_, 512){};  // AH20250819: Measured stack usage of 144 bytes (without ULOG errors)
+  thread_t *processing_thread_ = nullptr;
+
+  UARTDriver *uart_{};
+  UARTConfigEx uart_config_{};
+
+  void threadFunc();
+  static void threadHelper(void *instance) {
+    static_cast<YFR4escDriver *>(instance)->threadFunc();
+  };
+
+  void ProcessRxBytes(const volatile uint8_t *data, size_t len);
+  void ProcessDecodedPacket(uint8_t *packet, size_t len);
+
+  void SendControl(float duty);
+  void SendSettings();
+};
+}  // namespace xbot::driver::motor
+
+#endif  // YFR4ESCDRIVER_H