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 +#include +#include +#include +#include +#include +#include +#include +#include + +auto buffer = std::array{}; + +auto read_payload(std::span 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(¤t), 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 #include +#include struct HardwareSerial { @@ -14,6 +15,8 @@ struct HardwareSerial auto read() const -> std::uint8_t; + auto read(std::span 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(c); } +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto HardwareSerial::read(std::span 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 {