Sabo WIP

robots/src/Sabo/robot_ex.hpp

@@ -10,4 +10,13 @@
 #define LINE_EMERGENCY_3 LINE_GPIO11  // Top stop button (Hall)
 #define LINE_EMERGENCY_4 LINE_GPIO10  // Back-handle stop (Capacitive)
 
+// CoverUI
+#define SPI_UI SPID1                   // CoverUI is wired to SPI1
+#define LINE_UI_SCK LINE_SPI1_SCK      // Clock for HEF4794BT & 74HC165
+#define LINE_UI_MISO LINE_SPI1_MISO    // Button from 74HC165
+#define LINE_UI_MOSI LINE_SPI1_MOSI    // LED & Button matrix to HEF4794BT
+#define LINE_UI_LATCH_LOAD LINE_GPIO9  // HEF4794BT STR or 74HC165 /PL
+#define LINE_UI_LED_OE LINE_GPIO8      // HEF4794BT OE
+#define LINE_UI_BTN_CE LINE_GPIO1      // 74HC165 /CE
+
 #endif  // ROBOT_EX_HPP

robots/src/Sabo/sabo_robot.cpp

@@ -3,6 +3,7 @@
 //
 
 #include <drivers/motor/vesc/VescDriver.h>
+#include <ulog.h>
 
 #include <debug/debug_tcp_interface.hpp>
 #include <drivers/charger/bq_2576/bq_2576.hpp>
@@ -11,6 +12,9 @@
 #include <services.hpp>
 
 #include "robot.hpp"
+#include "sabo_ui_controller.hpp"
+#include "sabo_ui_driver.hpp"
+
 namespace Robot {
 
 static BQ2576 charger{};
@@ -23,6 +27,9 @@ static DebugTCPInterface left_esc_driver_interface_{65102, &left_motor_driver};
 static DebugTCPInterface mower_esc_driver_interface_{65103, &mower_motor_driver};
 static DebugTCPInterface right_esc_driver_interface_{65104, &right_motor_driver};
 
+static SaboUIDriver ui_driver;
+static SaboUIController ui(&ui_driver);
+
 namespace General {
 void InitPlatform() {
   // Front left wheel lift (Hall)
@@ -62,10 +69,20 @@ void InitPlatform() {
   mower_service.SetDriver(&mower_motor_driver);
   charger.setI2C(&I2CD1);
   power_service.SetDriver(&charger);
+
+  // UI
+  ui.start();
+  ui.setLED(SaboUIController::LEDID::START_RD, SaboUIController::LEDMode::BLINK_SLOW);
 }
 bool IsHardwareSupported() {
-  // FIXME: Fix EEPROM reading and check EEPROM
-  return true;
+  // Accept Sabo 0.1.x boards
+  if (strncmp("hw-openmower-sabo", carrier_board_info.board_id, sizeof(carrier_board_info.board_id)) == 0 &&
+      strncmp("xcore", board_info.board_id, sizeof(board_info.board_id)) == 0 && board_info.version_major == 1 &&
+      carrier_board_info.version_major == 0 && carrier_board_info.version_minor == 1) {
+    return true;
+  }
+
+  return false;
 }
 }  // namespace General
 

robots/src/Sabo/sabo_ui_controller.cpp

@@ -0,0 +1,151 @@
+//
+// Created by Apehaenger on 4/20/25.
+//
+#include "sabo_ui_controller.hpp"
+
+#include <ulog.h>
+
+static constexpr uint8_t EVT_PACKET_RECEIVED = 1;
+
+void SaboUIController::start() {
+  if (thread_ != nullptr) {
+    ULOG_ERROR("Started Sabo UI Controller twice!");
+    return;
+  }
+
+  if (driver_ == nullptr) {
+    ULOG_ERROR("Sabo UI Driver not set!");
+    return;
+  }
+  if (!driver_->init()) {
+    ULOG_ERROR("Failed to initialize Sabo UI Driver!");
+    return;
+  }
+
+  thread_ = chThdCreateStatic(&wa_, sizeof(wa_), NORMALPRIO, ThreadHelper, this);
+#ifdef USE_SEGGER_SYSTEMVIEW
+  processing_thread_->name = "SaboUIController";
+#endif
+
+  // Now that driver is initialized and thread got started, we can enable output
+  driver_->enableOutput();
+  chThdSleepMilliseconds(100);
+  playPowerOnAnimation();
+}
+
+void SaboUIController::ThreadHelper(void* instance) {
+  auto* i = static_cast<SaboUIController*>(instance);
+  i->ThreadFunc();
+}
+
+void SaboUIController::tick() {
+  const systime_t now = chVTGetSystemTime();
+
+  // Slow blink handling
+  if (leds_.slow_blink_mask && (now - leds_.last_slow_update >= TIME_MS2I(500))) {
+    leds_.last_slow_update = now;
+    leds_.slow_blink_state = !leds_.slow_blink_state;
+  }
+
+  // Fast blink handling
+  if (leds_.fast_blink_mask && (now - leds_.last_fast_update >= TIME_MS2I(100))) {
+    leds_.last_fast_update = now;
+    leds_.fast_blink_state = !leds_.fast_blink_state;
+  }
+
+  // Final LED state calculation
+  uint8_t active_leds = leds_.on_mask;
+  active_leds |= leds_.slow_blink_state ? leds_.slow_blink_mask : 0;
+  active_leds |= leds_.fast_blink_state ? leds_.fast_blink_mask : 0;
+
+  driver_->setLEDs(active_leds);
+  driver_->latchLoad();
+
+  // Button debouncing (of all buttons)
+  const uint16_t raw = driver_->getRawButtonStates();
+  const uint16_t changed_bits = btn_last_raw_ ^ raw;  // XOR to find changed bits
+  if (changed_bits == 0) {
+    if (btn_debounce_counter_ < DEBOUNCE_TICKS) btn_debounce_counter_++;
+  } else {
+    btn_debounce_counter_ = 0;
+  }
+  btn_stable_states_ = (btn_debounce_counter_ >= DEBOUNCE_TICKS) ? raw : btn_stable_states_;
+  btn_last_raw_ = raw;
+
+  // Debug buttons
+  /*static uint16_t last_reported_states_ = 0xFFFF;  // Last debug output
+  if (last_reported_states_ != btn_stable_states_) {
+    last_reported_states_ = btn_stable_states_;
+    ULOG_INFO("DEBUG: Buttons: 0x%04X (Raw: 0x%04X)", (~btn_stable_states_ & 0xFFFF), btn_stable_states_);
+  }*/
+
+  // Debug stack size
+  /*size_t stack_size = sizeof(wa_);
+  size_t unused_stack = (uint8_t*)thread_->ctx.sp - (uint8_t*)&wa_[0];
+  size_t used_stack = stack_size - unused_stack;
+  ULOG_INFO("DEBUG: Stack: %u/%u used", used_stack, stack_size);*/
+}
+
+void SaboUIController::setLED(LEDID id, LEDMode mode) {
+  const uint8_t bit = 1 << uint8_t(id);
+
+  // Clear existing state
+  leds_.on_mask &= ~bit;
+  leds_.slow_blink_mask &= ~bit;
+  leds_.fast_blink_mask &= ~bit;
+
+  // Set new state
+  switch (mode) {
+    case LEDMode::ON: leds_.on_mask |= bit; break;
+    case LEDMode::BLINK_SLOW: leds_.slow_blink_mask |= bit; break;
+    case LEDMode::BLINK_FAST: leds_.fast_blink_mask |= bit; break;
+    case LEDMode::OFF:
+    default: break;
+  }
+}
+
+void SaboUIController::playPowerOnAnimation() {
+  leds_.on_mask = 0x1F;  // All on
+  chThdSleepMilliseconds(400);
+  leds_.on_mask = 0x00;  // All off
+  chThdSleepMilliseconds(800);
+
+  // Knight Rider
+  for (int i = 0; i <= 5; i++) {
+    leds_.on_mask = (1 << i) - 1;
+    chThdSleepMilliseconds(100);
+  }
+  for (int i = 4; i >= 0; i--) {
+    leds_.on_mask = (1 << i) - 1;
+    chThdSleepMilliseconds(100);
+  }
+}
+
+void SaboUIController::ThreadFunc() {
+  while (true) {
+    tick();
+
+    // FIXME: Sample code to wait for some other event
+    eventmask_t event = chEvtWaitAnyTimeout(
+        EVT_PACKET_RECEIVED,
+        TIME_MS2I(1));  // FIXME: Once buttons are implemented, this needs to become something around 1ms
+    if (event & EVT_PACKET_RECEIVED) {
+      /*chSysLock();
+      // Forbid packet reception
+      processing = true;
+      chSysUnlock();
+      if (buffer_fill > 0) {
+        ProcessPacket();
+      }
+      buffer_fill = 0;
+      chSysLock();
+      // Allow packet reception
+      processing = false;
+      chSysUnlock();*/
+    }
+  }
+}
+
+bool SaboUIController::isButtonPressed(ButtonID btn) {
+  return (btn_stable_states_ & (1 << uint8_t(btn))) == 0;
+}

robots/src/Sabo/sabo_ui_controller.hpp

@@ -0,0 +1,58 @@
+//
+// Created by Apehaenger on 4/20/25.
+//
+
+#ifndef OPENMOWER_SABO_UI_CONTROLLER_HPP
+#define OPENMOWER_SABO_UI_CONTROLLER_HPP
+
+#include "ch.h"
+#include "sabo_ui_driver.hpp"
+
+class SaboUIController {
+ public:
+  explicit SaboUIController(SaboUIDriver* driver) : driver_(driver) {
+  }
+
+  static constexpr uint8_t DEBOUNCE_TICKS = 20;  // 20 * 2ms(tick) = 40ms debounce time
+
+  enum class LEDID : uint8_t { AUTO, MOWING, HOME, START_GN, START_RD };  // Same bits as connected to HEF4794BT
+  enum class LEDMode { OFF, ON, BLINK_SLOW, BLINK_FAST };
+  enum class ButtonID : uint8_t { UP = 0, DOWN, LEFT, RIGHT, OK, START, MENU = 8, BACK, AUTO, MOW, HOME };
+
+  void start();                         // Initializes the UI driver and starts the controller thread
+  void setLED(LEDID id, LEDMode mode);  // Set LED state
+
+  bool isButtonPressed(ButtonID btn);  // Debounced safe check if a button is pressed
+  // void setButtonCallback(std::function<void(ButtonID)> callback);
+
+  void playPowerOnAnimation();
+
+ private:
+  THD_WORKING_AREA(wa_, 1024);
+  thread_t* thread_ = nullptr;
+
+  SaboUIDriver* driver_;
+  // std::function<void(ButtonID)> button_callback_;
+
+  struct LEDState {
+    uint8_t on_mask = 0;
+    uint8_t slow_blink_mask = 0;
+    uint8_t fast_blink_mask = 0;
+    systime_t last_slow_update = 0;
+    systime_t last_fast_update = 0;
+    bool slow_blink_state = false;
+    bool fast_blink_state = false;
+  };
+  LEDState leds_;
+
+  uint16_t btn_last_raw_ = 0xFFFF;       // Last raw button state
+  uint16_t btn_stable_states_ = 0xFFFF;  // Stable (debounced) button state
+  uint8_t btn_debounce_counter_ = 0;     // If this counter is >= DEBOUNCE_TICKS, the button state is stable/debounced
+
+  static void ThreadHelper(void* instance);
+  void ThreadFunc();
+
+  void tick();
+};
+
+#endif  // OPENMOWER_SABO_UI_CONTROLLER_HPP

robots/src/Sabo/sabo_ui_driver.cpp

@@ -0,0 +1,92 @@
+//
+// Created by Apehaenger on 4/19/25.
+//
+#include "sabo_ui_driver.hpp"
+
+#include <ulog.h>
+
+#include "globals.hpp"
+#include "robot_ex.hpp"
+
+bool SaboUIDriver::init() {
+  // Init SPI pins
+  palSetLineMode(LINE_UI_SCK, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING);
+  palSetLineMode(LINE_UI_MISO, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING);
+  palSetLineMode(LINE_UI_MOSI, PAL_MODE_ALTERNATE(5) | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING);
+
+  // Init GPIOs for chip control
+  palSetLineMode(LINE_UI_LATCH_LOAD, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING);
+  palSetLineMode(LINE_UI_LED_OE, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 | PAL_STM32_PUPDR_FLOATING);
+  palSetLineMode(LINE_UI_BTN_CE,
+                 PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_MID2 |
+                     PAL_STM32_PUPDR_PULLDOWN);  // FIXME: CoverUI doesn't has any pull-up/down resistors?
+
+  // Configure SPI
+  spi_ = &SPI_UI;
+  spi_cfg_ = {
+      .circular = false,
+      .slave = false,
+      .data_cb = nullptr,
+      .error_cb = nullptr,
+      .ssline = 0,                                                     // Master mode
+      .cfg1 = SPI_CFG1_MBR_0 | SPI_CFG1_MBR_1 |                        // Baudrate = FPCLK/16 (~10 MHz @ 160 MHz SPI-C)
+              SPI_CFG1_DSIZE_2 | SPI_CFG1_DSIZE_1 | SPI_CFG1_DSIZE_0,  // 8-Bit (DS = 0b111)
+      .cfg2 = SPI_CFG2_MASTER                                          // Master mode
+  };
+
+  // Start SPI
+  if (spiStart(spi_, &spi_cfg_) != MSG_OK) {
+    ULOG_ERROR("Error starting UI-SPI");
+    return false;
+  }
+
+  return true;
+}
+
+void SaboUIDriver::latchLoad() {
+  uint8_t button_data = 0;
+  uint8_t tx_data = current_leds_ | ((current_button_row_ + 1) << 5);  // Bit5/6 for row0/1
+
+  // Load 74HC165 shift register (the very first load will load nothing, because row is not set yet. Who cares?)
+  palWriteLine(LINE_UI_LATCH_LOAD, PAL_LOW);  // /PL (parallel load) Pulse
+  chThdSleepMicroseconds(1);
+  palWriteLine(LINE_UI_LATCH_LOAD, PAL_HIGH);
+
+  // SPI transfer LEDs and read button for previously set row
+  spiAcquireBus(spi_);
+  palWriteLine(LINE_UI_LATCH_LOAD, PAL_HIGH);    // Latch HEF4794BT (STR = HIGH)
+  palWriteLine(LINE_UI_BTN_CE, PAL_LOW);         // HC165 /CE
+  spiExchange(spi_, 1, &tx_data, &button_data);  // Send data and receive button data
+  palWriteLine(LINE_UI_LATCH_LOAD, PAL_LOW);     // Stop latching (STR = LOW)
+  palWriteLine(LINE_UI_BTN_CE, PAL_HIGH);
+  spiReleaseBus(spi_);
+
+  // Buffer / Shift & buffer depending on current row
+  if (current_button_row_ == 0) {
+    button_states_ = (button_states_ & 0xFF00) | button_data;
+  } else {
+    button_states_ = (button_states_ & 0x00FF) | (button_data << 8);
+  }
+
+  // Alternate between row0 and row1
+  current_button_row_ ^= 1;
+}
+
+void SaboUIDriver::enableOutput() {
+  // TODO: OE could also be used with a PWM signal to dim the LEDs
+
+  if (carrier_board_info.version_major <= 0 && carrier_board_info.version_minor <= 1) {
+    // Sabo v0.1 has an HEF4794BT OE driver which inverts the signal. Newer boards will not have this driver anymore.
+    palWriteLine(LINE_UI_LED_OE, PAL_LOW);
+  } else {
+    palWriteLine(LINE_UI_BTN_CE, PAL_HIGH);
+  }
+}
+
+void SaboUIDriver::setLEDs(uint8_t leds) {
+  current_leds_ = leds & 0x1F;  // LEDs are on the lower 5 bits
+}
+
+uint16_t SaboUIDriver::getRawButtonStates() const {
+  return button_states_;
+}

robots/src/Sabo/sabo_ui_driver.hpp

@@ -0,0 +1,28 @@
+//
+// Created by Apehaenger on 4/19/25.
+//
+
+#ifndef OPENMOWER_SABO_UI_DRIVER_HPP
+#define OPENMOWER_SABO_UI_DRIVER_HPP
+
+#include "ch.h"
+#include "hal.h"
+
+class SaboUIDriver {
+ private:
+  SPIDriver* spi_;
+  SPIConfig spi_cfg_;
+
+  uint8_t current_leds_ = 0;
+  uint8_t current_button_row_ = 0;  // Alternating button rows
+  uint16_t button_states_ = 0;      // Bits 0-7: Row0, Bits 8-15: Row1. Low-active!
+
+ public:
+  bool init();                          // Init SPI and GPIOs
+  void latchLoad();                     // Latch LEDs as well as button-row, and load button columns
+  void enableOutput();                  // Enable output for HEF4794BT
+  uint16_t getRawButtonStates() const;  // Get the raw button states (0-15). Low-active!
+  void setLEDs(uint8_t leds);           // Set LEDs to a specific pattern
+};
+
+#endif  // OPENMOWER_SABO_UI_DRIVER_HPP