add messages

stm32-modules/flex-stacker/firmware/CMakeLists.txt

@@ -106,7 +106,7 @@ set_target_properties(
 
 
 find_program(ARM_GDB
-        arm-none-eabi-gdb-py
+        arm-none-eabi-gdb-py3
         PATHS "${CrossGCC_BINDIR}"
         NO_DEFAULT_PATH
         REQUIRED)

stm32-modules/flex-stacker/src/errors.cpp

@@ -15,6 +15,7 @@ const char* const SYSTEM_SERIAL_NUMBER_HAL_ERROR =
     "ERR302:system:HAL error, busy, or timeout\n";
 const char* const SYSTEM_EEPROM_ERROR =
     "ERR303:system:EEPROM communication error\n";
+const char* const TMC2160_READ_ERROR = "ERR901:TMC2160 driver read error\n";
 
 const char* const UNKNOWN_ERROR = "ERR-1:unknown error code\n";
 
@@ -34,6 +35,7 @@ auto errors::errorstring(ErrorCode code) -> const char* {
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_INVALID);
         HANDLE_CASE(SYSTEM_SERIAL_NUMBER_HAL_ERROR);
         HANDLE_CASE(SYSTEM_EEPROM_ERROR);
+        HANDLE_CASE(TMC2160_READ_ERROR);
     }
     return UNKNOWN_ERROR;
 }

stm32-modules/include/flex-stacker/flex-stacker/errors.hpp

@@ -18,6 +18,8 @@ enum class ErrorCode {
     SYSTEM_SERIAL_NUMBER_INVALID = 301,
     SYSTEM_SERIAL_NUMBER_HAL_ERROR = 302,
     SYSTEM_EEPROM_ERROR = 303,
+    // 9xx - TMC2160
+    TMC2160_READ_ERROR = 901,
 };
 
 auto errorstring(ErrorCode code) -> const char*;

stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp

@@ -1,7 +1,6 @@
 /*
 ** Definitions of valid gcodes understood by the flex-stacker; intended to work
-*with
-** the gcode parser in gcode_parser.hpp
+** with the gcode parser in gcode_parser.hpp
 */
 
 #pragma once
@@ -234,4 +233,57 @@ struct GetBoardRevision {
     }
 };
 
+struct GetTMCRegister {
+    MotorID motor_id;
+    uint8_t reg;
+
+    using ParseResult = std::optional<GetTMCRegister>;
+    static constexpr auto prefix = std::array{'M', '9', '2', '0', ' '};
+    static constexpr const char* response = "M920 OK\n";
+
+    template <typename InputIt, typename InLimit>
+        requires std::forward_iterator<InputIt> &&
+                 std::sized_sentinel_for<InputIt, InLimit>
+    static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
+        return write_string_to_iterpair(buf, limit, response);
+    }
+    template <typename InputIt, typename Limit>
+        requires std::forward_iterator<InputIt> &&
+                 std::sized_sentinel_for<Limit, InputIt>
+    static auto parse(const InputIt& input, Limit limit)
+        -> std::pair<ParseResult, InputIt> {
+        auto working = prefix_matches(input, limit, prefix);
+        if (working == input) {
+            return std::make_pair(ParseResult(), input);
+        }
+        MotorID motor_id = MotorID::MOTOR_Z;
+        if (*working == 'X') {
+            motor_id = MotorID::MOTOR_X;
+        } else if (*working == 'L') {
+            motor_id = MotorID::MOTOR_L;
+        }else if (*working != 'Z') {
+            return std::make_pair(ParseResult(), input);
+        }
+        working++;
+        if (working == limit) {
+            return std::make_pair(ParseResult(), input);
+        }
+
+        constexpr auto pref = std::array{' '};
+        auto before_pref = working;
+        working = prefix_matches(working, limit, pref);
+        if (working == before_pref) {
+            return std::make_pair(ParseResult(), input);
+        }
+
+        auto reg = parse_value<uint8_t>(working, limit);
+        if (!reg.first.has_value()) {
+            return std::make_pair(ParseResult(), input);
+        }
+        return std::make_pair(
+            ParseResult(GetTMCRegister{.motor_id = motor_id, .reg = reg}), reg.second);
+    }
+};
+
+
 }  // namespace gcode

stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp

@@ -37,15 +37,17 @@ class HostCommsTask {
     using Aggregator = typename tasks::Tasks<QueueImpl>::QueueAggregator;
 
   private:
+    using GCodeParser = gcode::GroupParser<gcode::GetTMCRegister>;
     using AckOnlyCache =
         AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber>;
     using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>;
+    using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>;
   public:
     static constexpr size_t TICKS_TO_WAIT_ON_SEND = 10;
     explicit HostCommsTask(Queue& q, Aggregator* aggregator)
         : message_queue(q), task_registry(aggregator),
           ack_only_cache(),
-          get_system_info_cache() {}
+          get_system_info_cache(), get_tmc_register_cache() {}
     HostCommsTask(const HostCommsTask& other) = delete;
     auto operator=(const HostCommsTask& other) -> HostCommsTask& = delete;
     HostCommsTask(HostCommsTask&& other) noexcept = delete;
@@ -208,21 +210,62 @@ class HostCommsTask {
     template <typename InputIt, typename InputLimit>
     requires std::forward_iterator<InputIt> &&
         std::sized_sentinel_for<InputLimit, InputIt>
-    auto visit_message(const messages::GetMotorDriverRegisterResponse& msg,
+    auto visit_message(const messages::GetTMCRegisterResponse& response,
                        InputIt tx_into, InputLimit tx_limit) -> InputIt {
-        static_cast<void>(msg);
+        auto cache_entry = get_tmc_register_cache.remove_if_present(
+            response.responding_to_id);
+        return std::visit(
+            [tx_into, tx_limit, response](auto cache_element) {
+                using T = std::decay_t<decltype(cache_element)>;
+                if constexpr (std::is_same_v<std::monostate, T>) {
+                    return errors::write_into(
+                        tx_into, tx_limit,
+                        errors::ErrorCode::BAD_MESSAGE_ACKNOWLEDGEMENT);
+                } else {
+                    return cache_element.write_response_into(
+                        tx_into, tx_limit);
+                }
+            },
+            cache_entry);
+    }
+
+    template <typename InputIt, typename InputLimit>
+        requires std::forward_iterator<InputIt> &&
+                 std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const std::monostate& ignore, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        static_cast<void>(ignore);
         static_cast<void>(tx_into);
         static_cast<void>(tx_limit);
-        return tx_into;
+        return std::make_pair(true, tx_into);
     }
 
-
-
+    template <typename InputIt, typename InputLimit>
+        requires std::forward_iterator<InputIt> &&
+                 std::sized_sentinel_for<InputLimit, InputIt>
+    auto visit_gcode(const gcode::GetTMCRegister& gcode, InputIt tx_into,
+                     InputLimit tx_limit) -> std::pair<bool, InputIt> {
+        auto id = get_tmc_register_cache.add(gcode);
+        if (id == 0) {
+            return std::make_pair(
+                false, errors::write_into(tx_into, tx_limit,
+                                          errors::ErrorCode::GCODE_CACHE_FULL));
+        }
+        auto message = messages::GetTMCRegisterMessage{.id = id, .motor_id = gcode.motor_id, .reg = gcode.reg};
+        if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
+            auto wrote_to = errors::write_into(
+                tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
+            get_tmc_register_cache.remove_if_present(id);
+            return std::make_pair(false, wrote_to);
+        }
+        return std::make_pair(true, tx_into);
+    }
 
     Queue& message_queue;
     Aggregator* task_registry;
     AckOnlyCache ack_only_cache;
     GetSystemInfoCache get_system_info_cache;
+    GetTMCRegisterCache get_tmc_register_cache;
     bool may_connect_latch = true;
 };
 

stm32-modules/include/flex-stacker/flex-stacker/messages.hpp

@@ -89,30 +89,30 @@ struct ForceUSBDisconnect {
     size_t return_address;
 };
 
-struct SetMotorDriverRegister {
+struct SetTMCRegisterMessage {
     uint32_t id;
     MotorID motor_id;
     uint8_t reg;
     uint32_t data;
 };
 
-struct GetMotorDriverRegister {
+struct GetTMCRegisterMessage {
     uint32_t id;
     MotorID motor_id;
     uint8_t reg;
 };
 
-struct PollMotorDriverRegister {
+struct PollTMCRegisterMessage {
     uint32_t id;
     MotorID motor_id;
     uint8_t reg;
 };
 
-struct StopPollingMotorDriverRegister {
+struct StopPollTMCRegisterMessage {
     uint32_t id;
 };
 
-struct GetMotorDriverRegisterResponse {
+struct GetTMCRegisterResponse {
     uint32_t responding_to_id;
     MotorID motor_id;
     uint8_t reg;
@@ -122,16 +122,16 @@ struct GetMotorDriverRegisterResponse {
 using HostCommsMessage =
     ::std::variant<std::monostate, IncomingMessageFromHost, ForceUSBDisconnect,
                    ErrorMessage, AcknowledgePrevious, GetSystemInfoResponse,
-                   GetMotorDriverRegisterResponse>;
+                   GetTMCRegisterResponse>;
 
 using SystemMessage =
     ::std::variant<std::monostate, AcknowledgePrevious, GetSystemInfoMessage,
                    SetSerialNumberMessage, EnterBootloaderMessage>;
 
 using MotorDriverMessage =
-    ::std::variant<std::monostate, SetMotorDriverRegister,
-                   GetMotorDriverRegister, PollMotorDriverRegister,
-                   StopPollingMotorDriverRegister>;
+    ::std::variant<std::monostate, SetTMCRegisterMessage,
+                   GetTMCRegisterMessage, PollTMCRegisterMessage,
+                   StopPollTMCRegisterMessage>;
 using MotorMessage = ::std::variant<std::monostate>;
 
 };  // namespace messages

stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp

@@ -12,6 +12,7 @@
 #include "core/version.hpp"
 #include "firmware/motor_policy.hpp"
 #include "flex-stacker/messages.hpp"
+#include "flex-stacker/errors.hpp"
 #include "flex-stacker/tasks.hpp"
 #include "flex-stacker/tmc2160.hpp"
 #include "flex-stacker/tmc2160_interface.hpp"
@@ -119,8 +120,8 @@ class MotorDriverTask {
         if (!_task_registry) {
             return;
         }
+        auto tmc2160_interface = tmc2160::TMC2160Interface<Policy>(policy);
         if (!_initialized) {
-            static tmc2160::TMC2160Interface<Policy> tmc2160_interface(policy);
             if (!_tmc2160.initialize_config(motor_x_config, tmc2160_interface,
                                             MotorID::MOTOR_X)) {
                 return;
@@ -139,35 +140,58 @@ class MotorDriverTask {
         auto message = Message(std::monostate());
 
         _message_queue.recv(&message);
-        auto visit_helper = [this](auto& message) -> void {
-            this->visit_message(message);
+        auto visit_helper = [this, &tmc2160_interface](auto& message) -> void {
+            this->visit_message(message, tmc2160_interface);
         };
         std::visit(visit_helper, message);
     }
 
   private:
-    auto visit_message(const std::monostate& message) -> void {
-        static_cast<void>(message);
+    template <tmc2160::TMC2160InterfacePolicy Policy>
+    auto visit_message(const std::monostate& m, tmc2160::TMC2160Interface<Policy>& tmc2160_interface) -> void {
+        static_cast<void>(m);
+        static_cast<void>(tmc2160_interface);
     }
 
-    auto visit_message(const messages::SetMotorDriverRegister& message)
+    template <tmc2160::TMC2160InterfacePolicy Policy>
+    auto visit_message(const messages::SetTMCRegisterMessage& m, tmc2160::TMC2160Interface<Policy>& tmc2160_interface)
         -> void {
-        static_cast<void>(message);
+        static_cast<void>(m);
+        static_cast<void>(tmc2160_interface);
     }
 
-    auto visit_message(const messages::GetMotorDriverRegister& message)
+    template <tmc2160::TMC2160InterfacePolicy Policy>
+    auto visit_message(const messages::GetTMCRegisterMessage& m,  tmc2160::TMC2160Interface<Policy>& tmc2160_interface)
         -> void {
-        static_cast<void>(message);
+        messages::HostCommsMessage response;
+        if (tmc2160::is_valid_address(m.reg)) {
+            auto data = tmc2160_interface.read(tmc2160::Registers(m.reg), m.motor_id);
+            if (!data.has_value()) {
+                response = messages::ErrorMessage{
+                    .code = errors::ErrorCode::TMC2160_READ_ERROR};
+            } else {
+                response = messages::GetTMCRegisterResponse{
+                    .responding_to_id = m.id,
+                    .motor_id = m.motor_id,
+                    .reg = m.reg,
+                    .data = data.value()};
+            }
+
+            static_cast<void>(
+                _task_registry->send_to_address(response, Queues::HostCommsAddress));
+        }
     }
 
-    auto visit_message(const messages::PollMotorDriverRegister& message)
+    template <tmc2160::TMC2160InterfacePolicy Policy>
+    auto visit_message(const messages::PollTMCRegisterMessage& m, tmc2160::TMC2160Interface<Policy>& tmc2160_interface)
         -> void {
-        static_cast<void>(message);
+        static_cast<void>(m);
     }
 
-    auto visit_message(const messages::StopPollingMotorDriverRegister& message)
+    template <tmc2160::TMC2160InterfacePolicy Policy>
+    auto visit_message(const messages::StopPollTMCRegisterMessage& m, tmc2160::TMC2160Interface<Policy>& tmc2160_interface)
         -> void {
-        static_cast<void>(message);
+        static_cast<void>(m);
     }
 
     Queue& _message_queue;