From 0b4b19a1235df9da71cd9f4f0aa3cec93558d194 Mon Sep 17 00:00:00 2001
From: Oliver Lee <oliverzlee@gmail.com>
Date: Thu, 27 Jul 2023 21:32:55 +0200
Subject: [PATCH] wip test read only rpm

Change-Id: Ie80b95b50b330f7f79680f9b854e2321ba13797c
---
 examples/vesc_cmd.cpp               |  8 +++++++-
 third_party/vesc_uart/src/extra.cpp | 13 +++++++++++--
 2 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/examples/vesc_cmd.cpp b/examples/vesc_cmd.cpp
index 6f22a32..1912527 100644
--- a/examples/vesc_cmd.cpp
+++ b/examples/vesc_cmd.cpp
@@ -7,6 +7,11 @@
 #include <fmt/core.h>
 #include <thread>
 
+#include <array>
+#include <cstdint>
+
+auto payload_buffer = std::array<std::uint8_t, 256>{};
+
 auto main() -> int
 {
   const auto current = -10.0F;
@@ -16,7 +21,8 @@ auto main() -> int
     fmt::print("current: {}\n", std::int16_t(current));
 
     const auto start = std::chrono::steady_clock::now();
-    fmt::print("rpm: {}\n", ::vesc::extra::read_rpm());
+    //fmt::print("rpm: {}\n", ::vesc::extra::read_rpm());
+    ::vesc::extra::read_payload(std::span{payload_buffer});
 
     const auto elapsed = std::chrono::steady_clock::now() - start;
     fmt::print("{}\n", std::chrono::duration<float>{elapsed}.count());
diff --git a/third_party/vesc_uart/src/extra.cpp b/third_party/vesc_uart/src/extra.cpp
index b69fccb..4e73b01 100644
--- a/third_party/vesc_uart/src/extra.cpp
+++ b/third_party/vesc_uart/src/extra.cpp
@@ -8,13 +8,20 @@
 #include <cstddef>
 #include <cstring>
 
+#include <iostream>
+
 namespace {
 
 static constexpr auto serial_device = 0;
 
 auto payload_buffer = std::array<std::uint8_t, 256>{};
 
-auto cmd_get_values = std::uint8_t{COMM_GET_VALUES};
+// try to use COMM_GET_VALUES_SELECTIVE
+constexpr std::uint8_t rpm_bit = 1U << 7;
+auto cmd_get_values = std::array<std::uint8_t, 5>{
+  COMM_GET_VALUES, rpm_bit, 0x00, 0x00, 0x00
+};
+// auto cmd_get_values = std::uint8_t{COMM_GET_VALUES, 0xFF, 0xFF, 0xFF, 0xFF};
 
 }  // namespace
 
@@ -24,7 +31,7 @@ namespace vesc::extra {
 
 auto read_payload(std::span<std::uint8_t> buf) -> void
 {
-  ::PackSendPayload(&cmd_get_values, sizeof(cmd_get_values), serial_device);
+  ::PackSendPayload(cmd_get_values.data(), sizeof(cmd_get_values), serial_device);
 
   {
     [[maybe_unused]] const auto version = Serial.read();
@@ -39,6 +46,8 @@ auto read_payload(std::span<std::uint8_t> buf) -> void
       "`buf` is not large enough to hold "
       "message");
 
+  std::cout << "payload size: " << static_cast<int>(payload_size) << '\n';
+
   const auto message =
       std::span{buf.data(), std::size_t(payload_size + footer_size)};
   Serial.read(message);