wip

Author: ClemensElflein

src/drivers/ui/YardForceCoverUI/yard_force_cover_ui_driver.cpp

@@ -7,7 +7,10 @@
 #include <sys/unistd.h>
 #include <ulog.h>
 
+#include <services.hpp>
+
 #include "COBS.h"
+#include "services/mower_ui_service/mower_ui_service.hpp"
 #include "ui_board.h"
 
 static constexpr uint8_t EVT_PACKET_RECEIVED = 1;
@@ -46,11 +49,11 @@ void YardForceCoverUIDriver::UartRxChar(UARTDriver *driver, uint16_t data) {
   YardForceCoverUIDriver *instance = reinterpret_cast<const UARTConfigEx *>(driver->config)->context;
   chDbgAssert(instance != nullptr, "instance cannot be null!");
   chSysLockFromISR();
-  if (instance->processing) {
+  if (instance->processing_) {
     chSysUnlockFromISR();
     return;
   }
-  instance->buffer[instance->buffer_fill++] = data;
+  instance->buffer_[instance->buffer_fill_++] = data;
   if (data == 0) {
     chEvtSignalI(instance->thread_, EVT_PACKET_RECEIVED);
   }
@@ -60,36 +63,133 @@ void YardForceCoverUIDriver::ThreadFunc() {
   while (true) {
     bool timeout = chEvtWaitAnyTimeout(EVT_PACKET_RECEIVED, TIME_S2I(1)) == 0;
     if (timeout) {
-      RequestFWVersion();
-
-      msg_set_leds msg{};
-      msg.type = Set_LEDs;
-      static double value = 0.0f;
-      setBars7(msg, value);
-      value += 0.1f;
-      sendUIMessage(&msg, sizeof(msg));
+      if (!board_found_) {
+        // Try to find the board
+        RequestFWVersion();
+      } else {
+        UpdateUILeds();
+      }
 
       continue;
     }
     chSysLock();
     // Forbid packet reception
-    processing = true;
+    processing_ = true;
     chSysUnlock();
-    if (buffer_fill > 0) {
+    if (buffer_fill_ > 0) {
       ProcessPacket();
     }
-    buffer_fill = 0;
+    buffer_fill_ = 0;
     chSysLock();
     // Allow packet reception
-    processing = false;
+    processing_ = false;
     chSysUnlock();
   }
 }
+
+void YardForceCoverUIDriver::UpdateUILeds() {
+  // Show Info Docking LED
+  msg_set_leds leds_message{};
+  leds_message.type = Set_LEDs;
+
+  float charging_current = power_service.GetChargeCurrent();
+  float adapter_volts = power_service.GetAdapterVolts();
+  float battery_percent = power_service.GetBatteryPercent();
+  float gps_quality_percent = mower_ui_service.GetGpsQuality();
+  const auto high_level_state = mower_ui_service.GetHighLevelState();
+  bool emergency = emergency_service.GetEmergency();
+
+  if ((charging_current > 0.80f) && (adapter_volts > 10.0f))
+    setLed(leds_message, LED_CHARGING, LED_blink_fast);
+  else if ((charging_current <= 0.80f) && (charging_current >= 0.15f) && (adapter_volts > 20.0f))
+    setLed(leds_message, LED_CHARGING, LED_blink_slow);
+  else if ((charging_current < 0.15f) && (adapter_volts > 20.0f))
+    setLed(leds_message, LED_CHARGING, LED_on);
+  else
+    setLed(leds_message, LED_CHARGING, LED_off);
+
+  // Show Info Battery state
+  if (battery_percent >= 0.1)
+    setLed(leds_message, LED_BATTERY_LOW, LED_off);
+  else
+    setLed(leds_message, LED_BATTERY_LOW, LED_on);
+
+  if (adapter_volts < 10.0f)  // activate only when undocked
+  {
+    // use the first LED row as bargraph
+    setBars7(leds_message, battery_percent);
+    if (gps_quality_percent == 0) {
+      // if quality is 0, flash all LEDs to notify the user to calibrate.
+      setBars4(leds_message, -1.0);
+    } else {
+      setBars4(leds_message, gps_quality_percent);
+    }
+  } else {
+    setBars7(leds_message, 0);
+    setBars4(leds_message, 0);
+  }
+
+  if (gps_quality_percent < 0.25) {
+    setLed(leds_message, LED_POOR_GPS, LED_on);
+  } else if (gps_quality_percent < 0.50) {
+    setLed(leds_message, LED_POOR_GPS, LED_blink_fast);
+  } else if (gps_quality_percent < 0.75) {
+    setLed(leds_message, LED_POOR_GPS, LED_blink_slow);
+  } else {
+    setLed(leds_message, LED_POOR_GPS, LED_off);
+  }
+
+  // Let S1 show if ros is connected and which state it's in
+  if (high_level_state == MowerUiService::HighLevelState::MODE_UNKNOWN) {
+    setLed(leds_message, LED_S1, LED_off);
+  } else {
+    switch (high_level_state) {
+      case MowerUiService::HighLevelState::MODE_IDLE:
+        setLed(leds_message, LED_S1, LED_on);
+        setLed(leds_message, LED_S2, LED_off);
+        break;
+      case MowerUiService::HighLevelState::MODE_AUTONOMOUS_MOWING:
+        setLed(leds_message, LED_S1, LED_blink_slow);
+        setLed(leds_message, LED_S2, LED_off);
+        break;
+      case MowerUiService::HighLevelState::MODE_AUTONOMOUS_DOCKING:
+        setLed(leds_message, LED_S1, LED_blink_slow);
+        setLed(leds_message, LED_S2, LED_blink_slow);
+        break;
+      case MowerUiService::HighLevelState::MODE_AUTONOMOUS_UNDOCKING:
+        setLed(leds_message, LED_S1, LED_blink_slow);
+        setLed(leds_message, LED_S2, LED_blink_fast);
+        break;
+      case MowerUiService::HighLevelState::MODE_RECORDING_OUTLINE:
+        setLed(leds_message, LED_S1, LED_blink_fast);
+        setLed(leds_message, LED_S2, LED_blink_slow);
+        break;
+      case MowerUiService::HighLevelState::MODE_RECORDING_OBSTACLE:
+        setLed(leds_message, LED_S1, LED_blink_fast);
+        setLed(leds_message, LED_S2, LED_blink_fast);
+        break;
+      default:
+        setLed(leds_message, LED_S1, LED_blink_fast);
+        setLed(leds_message, LED_S2, LED_on);
+        break;
+    }
+  }
+
+  // Show Info mower lifted or stop button pressed
+  if (emergency) {
+    setLed(leds_message, LED_MOWER_LIFTED, LED_blink_fast);
+  } else {
+    setLed(leds_message, LED_MOWER_LIFTED, LED_off);
+  }
+
+  sendUIMessage(&leds_message, sizeof(leds_message));
+}
+
 void YardForceCoverUIDriver::ProcessPacket() {
-  if (buffer_fill == 0) return;
-  uint16_t size = cobs_.decode((uint8_t *)buffer, buffer_fill - 1, encode_decode_buf_);
+  if (buffer_fill_ == 0) return;
+  uint16_t size = COBS::decode(const_cast<uint8_t *>(buffer_), buffer_fill_ - 1, encode_decode_buf_);
 
-  u_int16_t *crc_pointer = (uint16_t *)(encode_decode_buf_ + (size - 2));
+  auto *crc_pointer = reinterpret_cast<uint16_t *>(encode_decode_buf_ + (size - 2));
   u_int16_t readcrc = *crc_pointer;
 
   // check structure size
@@ -105,23 +205,22 @@ void YardForceCoverUIDriver::ProcessPacket() {
 
   if (crc != readcrc) return;
 
-  /*if (buffer[0] == Get_Version && size == sizeof(struct msg_get_version)) {
-    struct msg_get_version *msg = (struct msg_get_version *)buffer;
-    board_found = true;
-  } else if (buffer[0] == Get_Button && size == sizeof(struct msg_event_button)) {
-    struct msg_event_button *msg = (struct msg_event_button *)buffer;
-
-  } else if (buffer[0] == Get_Emergency && size == sizeof(struct msg_event_emergency)) {
-    struct msg_event_emergency *msg = (struct msg_event_emergency *)buffer;
-    stock_ui_emergency_state = msg->state;
-  } else if (buffer[0] == Get_Rain && size == sizeof(struct msg_event_rain)) {
-    struct msg_event_rain *msg = (struct msg_event_rain *)buffer;
-    stock_ui_rain = (msg->value < llhl_config.rain_threshold);
-  } else if (buffer[0] == Get_Subscribe && size == sizeof(struct msg_event_subscribe)) {
-    struct msg_event_subscribe *msg = (struct msg_event_subscribe *)buffer;
-    ui_topic_bitmask = msg->topic_bitmask;
-    ui_interval = msg->interval;
-  }*/
+  if (encode_decode_buf_[0] == Get_Version && size == sizeof(struct msg_get_version)) {
+    board_found_ = true;
+  } else if (encode_decode_buf_[0] == Get_Button && size == sizeof(struct msg_event_button)) {
+    msg_event_button *msg = (struct msg_event_button *)encode_decode_buf_;
+    mower_ui_service.SendButtonPressedEvent(msg->button_id);
+  } /* else if (encode_decode_buf_[0] == Get_Emergency && size == sizeof(struct msg_event_emergency)) {
+     struct msg_event_emergency *msg = (struct msg_event_emergency *)encode_decode_buf_;
+     stock_ui_emergency_state = msg->state;
+   } else if (encode_decode_buf_[0] == Get_Rain && size == sizeof(struct msg_event_rain)) {
+     struct msg_event_rain *msg = (struct msg_event_rain *)encode_decode_buf_;
+     stock_ui_rain = (msg->value < llhl_config.rain_threshold);
+   } else if (encode_decode_buf_[0] == Get_Subscribe && size == sizeof(struct msg_event_subscribe)) {
+     struct msg_event_subscribe *msg = (struct msg_event_subscribe *)encode_decode_buf_;
+     ui_topic_bitmask = msg->topic_bitmask;
+     ui_interval = msg->interval;
+   }*/
 }
 void YardForceCoverUIDriver::sendUIMessage(void *msg, size_t size) {
   // packages need to be at least 1 byte of type, 1 byte of data and 2 bytes of CRC
@@ -137,12 +236,12 @@ void YardForceCoverUIDriver::sendUIMessage(void *msg, size_t size) {
   data_pointer[size - 1] = (crc >> 8) & 0xFF;
   data_pointer[size - 2] = crc & 0xFF;
 
-  if (cobs_.getEncodedBufferSize(size) >= sizeof(encode_decode_buf_)) {
+  if (COBS::getEncodedBufferSize(size) >= sizeof(encode_decode_buf_)) {
     ULOG_ERROR("out_but_ size too small!");
     return;
   }
 
-  size_t encoded_size = cobs_.encode(data_pointer, size, encode_decode_buf_);
+  size_t encoded_size = COBS::encode(data_pointer, size, encode_decode_buf_);
   encode_decode_buf_[encoded_size] = 0;
   encoded_size++;
   uartSendFullTimeout(uart_, &encoded_size, encode_decode_buf_, TIME_INFINITE);

src/drivers/ui/YardForceCoverUI/yard_force_cover_ui_driver.hpp

@@ -9,7 +9,6 @@
 
 #include <drivers/input/input_driver.hpp>
 
-#include "COBS.h"
 #include "ch.h"
 #include "hal.h"
 #include "ui_board.h"
@@ -27,19 +26,20 @@ class YardForceCoverUIDriver {
   UARTConfigEx uart_config_{};
   etl::crc16_ccitt CRC16{};
 
-  volatile uint8_t buffer[255];
-  volatile uint8_t buffer_fill = 0;
-  volatile bool processing = false;
+  volatile uint8_t buffer_[255];
+  volatile uint8_t buffer_fill_ = 0;
+  volatile bool processing_ = false;
 
-  bool board_found = false;
+  bool board_found_ = false;
   uint8_t encode_decode_buf_[100]{};
-  COBS cobs_{};
 
   static void ThreadHelper(void *instance);
   static void UartRxChar(UARTDriver *driver, uint16_t data);
 
   void ThreadFunc();
 
+  void UpdateUILeds();
+
   void ProcessPacket();
 
   void sendUIMessage(void *msg, size_t size);

src/services/mower_ui_service/mower_ui_service.hpp

@@ -5,54 +5,79 @@
 #ifndef OPENMOWER_MOWER_UI_SERVICE_HPP
 #define OPENMOWER_MOWER_UI_SERVICE_HPP
 
+#include <etl/string.h>
+
 #include <MowerUiServiceBase.hpp>
+#include <xbot-service/Lock.hpp>
 
 #include "globals.hpp"
-#include <etl/string.h>
-#include <xbot-service/Lock.hpp>
 
 using namespace xbot::service;
 
+#define HL_SUBMODE_SHIFT 6
+
 class MowerUiService : public MowerUiServiceBase {
  private:
   THD_WORKING_AREA(wa, 1024){};
 
  public:
+  enum class HighLevelState : uint8_t {
+    // UNKNOWN (0)
+    MODE_UNKNOWN = 0,
+
+    // IDLE mode (1) and submodes
+    MODE_IDLE = 1,
+
+    // AUTONOMOUS mode (2) and submodes
+    MODE_AUTONOMOUS = 2,
+    MODE_AUTONOMOUS_MOWING = (0 << HL_SUBMODE_SHIFT) | MODE_AUTONOMOUS,
+    MODE_AUTONOMOUS_DOCKING = (1 << HL_SUBMODE_SHIFT) | MODE_AUTONOMOUS,
+    MODE_AUTONOMOUS_UNDOCKING = (2 << HL_SUBMODE_SHIFT) | MODE_AUTONOMOUS,
+
+    // RECORDING mode (3) and submodes
+    MODE_RECORDING = 3,
+    MODE_RECORDING_OUTLINE = (1 << HL_SUBMODE_SHIFT) | MODE_RECORDING,
+    MODE_RECORDING_OBSTACLE = (2 << HL_SUBMODE_SHIFT) | MODE_RECORDING,
+  };
+
   explicit MowerUiService(uint16_t service_id) : MowerUiServiceBase(service_id, wa, sizeof(wa)) {
   }
-  
-  [[nodiscard]] HighLevelStatus getStateId() { 
+
+  [[nodiscard]] HighLevelState GetHighLevelState() {
     xbot::service::Lock lk{&mtx_};
-    return state_id_; 
+    return static_cast<HighLevelState>(state_id_);
   }
-  [[nodiscard]] etl::string<100> getStateName() {
+  [[nodiscard]] etl::string<100> GetStateName() {
     xbot::service::Lock lk{&mtx_};
     return state_name_;
   }
-  [[nodiscard]] etl::string<100> getSubStateName() {
+  [[nodiscard]] etl::string<100> GetSubStateName() {
     xbot::service::Lock lk{&mtx_};
     return sub_state_name_;
   }
-  [[nodiscard]] float getGpsQuality() {
+  [[nodiscard]] float GetGpsQuality() {
     xbot::service::Lock lk{&mtx_};
     return gps_quality_;
   }
-  [[nodiscard]] int16_t getCurrentArea() {
+  [[nodiscard]] int16_t GetCurrentArea() {
     xbot::service::Lock lk{&mtx_};
     return current_area_;
   }
-  [[nodiscard]] int16_t getCurrentPath() {
+  [[nodiscard]] int16_t GetCurrentPath() {
     xbot::service::Lock lk{&mtx_};
     return current_path_;
   }
-  [[nodiscard]] int16_t getCurrentPathIndex() {
+  [[nodiscard]] int16_t GetCurrentPathIndex() {
     xbot::service::Lock lk{&mtx_};
     return current_path_index_;
   }
 
-  void SendAction(HighLevelAction action);
+  void SendButtonPressedEvent(uint8_t button_id) {
+    xbot::service::Lock lk{&mtx_};
+    MowerUiServiceBase::SendButtonPressedEvent(button_id);
+  }
 
-  void SetCallback(const etl::delegate<void()> &callback) {
+  void SetCallback(const etl::delegate<void()>& callback) {
     xbot::service::Lock lk{&mtx_};
     state_changed_callback_ = callback;
   }