From fb5e1d3092d24b9084707d717d2faf2f30655108 Mon Sep 17 00:00:00 2001
From: Oliver Lee <oliverzlee@gmail.com>
Date: Thu, 27 Jul 2023 16:24:36 +0200
Subject: [PATCH] Read values from VESC motor controller

Change-Id: Icaad5bd270827f1fa1b22a77d0cd0e23dbf999e5
---
 examples/BUILD.bazel                    |  5 +-
 examples/vesc_cmd.cpp                   | 65 ++++++++++++++++++++++++-
 uart_abstraction/HardwareSerial.hpp     |  5 +-
 uart_abstraction/rpi/HardwareSerial.cpp | 11 +++++
 4 files changed, 83 insertions(+), 3 deletions(-)

diff --git a/examples/BUILD.bazel b/examples/BUILD.bazel
index bfa9b66..74faa58 100644
--- a/examples/BUILD.bazel
+++ b/examples/BUILD.bazel
@@ -3,7 +3,10 @@ load("@rules_cc//cc:defs.bzl", "cc_binary")
 cc_binary(
     name = "vesc_cmd",
     srcs = ["vesc_cmd.cpp"],
-    deps = ["//third_party/vesc_uart"],
+    deps = [
+        "//third_party/vesc_uart",
+        "@fmt",
+    ],
 )
 
 cc_binary(
diff --git a/examples/vesc_cmd.cpp b/examples/vesc_cmd.cpp
index 8e40f4b..b6247d5 100644
--- a/examples/vesc_cmd.cpp
+++ b/examples/vesc_cmd.cpp
@@ -1,8 +1,71 @@
 #include "third_party/vesc_uart/VescUart.h"
+//#include ""
+
+#include <cstdint>
+#include <fmt/core.h>
+#include <fmt/ranges.h>
+#include <chrono>
+#include <thread>
+#include <array>
+#include <span>
+#include <cstring>
+#include <bit>
+
+auto buffer = std::array<std::uint8_t, 256>{};
+
+auto read_payload(std::span<std::uint8_t> buf) -> void
+{
+  static constexpr auto serial_device = 0;
+
+  static auto cmd_get_values = std::uint8_t{ COMM_GET_VALUES };
+  ::PackSendPayload(&cmd_get_values, sizeof(cmd_get_values), serial_device);
+
+  {
+    const auto version = Serial.read();
+    assert(version == 2 and "only version 2 is handled");
+  }
+
+  const auto payload_size = Serial.read();
+  static constexpr auto footer_size = 3;
+
+  fmt::print("payload_size: {}\n", payload_size);
+
+  const auto message = std::span{buf.data(), std::size_t(payload_size + footer_size)};
+  Serial.read(message);
+
+  const auto front = std::span{buf.data(),
+                               //2 + 2 + 4 + 4 + 4 + 4 + 2 + 4
+                               30
+  };
+  fmt::print("{::0<2X}\n", front);
+
+  const auto rpm_buf = std::span{buf.data() + 23,
+                               //2 + 2 + 4 + 4 + 4 + 4 + 2 + 4
+                               4};
+  fmt::print("{::0<2X}\n", rpm_buf);
+
+  auto rpm = std::int32_t{};
+  std::memcpy(&rpm, rpm_buf.data(), rpm_buf.size());
+
+  fmt::print("{}\n", std::byteswap(rpm));
+
+  // https://github.com/vedderb/bldc/blob/master/comm/commands.c#L379
+}
 
 auto main() -> int
 {
-  ::VescUartSetCurrent(1.0F);
+  const auto current = 10.0F;
+  //const auto data = std::span{reinterpret_cast<const std::uint8_t*>(&current), sizeof(current)};
+
+  while (true) {
+    //::VescUartSetCurrent(current += 1.0F);
+    ::VescUartSetCurrent(current);
+    fmt::print("current: {}\n", std::int16_t(current));
+
+    read_payload({buffer.data(), buffer.size()});
+
+    std::this_thread::sleep_for(std::chrono::milliseconds{100});
+  }
 
   return 0;
 }
diff --git a/uart_abstraction/HardwareSerial.hpp b/uart_abstraction/HardwareSerial.hpp
index 0a5af4d..e505789 100644
--- a/uart_abstraction/HardwareSerial.hpp
+++ b/uart_abstraction/HardwareSerial.hpp
@@ -1,9 +1,10 @@
-// Fake API for UART read/write.
+// API for UART read/write.
 
 #pragma once
 
 #include <cstddef>
 #include <cstdint>
+#include <span>
 
 struct HardwareSerial
 {
@@ -14,6 +15,8 @@ struct HardwareSerial
 
   auto read() const -> std::uint8_t;
 
+  auto read(std::span<std::uint8_t> buffer) const -> void;
+
   auto write(std::uint8_t* buffer, std::size_t size) const -> void;
 };
 
diff --git a/uart_abstraction/rpi/HardwareSerial.cpp b/uart_abstraction/rpi/HardwareSerial.cpp
index 2107bd7..94b7016 100644
--- a/uart_abstraction/rpi/HardwareSerial.cpp
+++ b/uart_abstraction/rpi/HardwareSerial.cpp
@@ -65,6 +65,17 @@ auto HardwareSerial::read() const -> std::uint8_t
   return static_cast<std::uint8_t>(c);
 }
 
+// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
+auto HardwareSerial::read(std::span<std::uint8_t> buf) const -> void
+{
+  auto buf_ = asio::buffer(buf.data(), buf.size());
+
+  while (buf_.size() != 0) {
+    // NOLINTNEXTLINE(bugprone-unchecked-optional-access)
+    buf_ += underlying->read_some(buf_);
+  }
+}
+
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
 auto HardwareSerial::write(std::uint8_t* buffer, std::size_t size) const -> void
 {