From 47241d9a326a16486855eae5c1b30d372558f6fe Mon Sep 17 00:00:00 2001 From: vegano1 Date: Mon, 16 Dec 2024 13:33:42 -0500 Subject: [PATCH] add SetStatusBarColor gcodes and implementation --- .../firmware/system/i2c_hardware.c | 19 ---- .../flex-stacker/firmware/ui/ui_policy.cpp | 1 + stm32-modules/flex-stacker/src/errors.cpp | 3 + .../flex-stacker/firmware/ui_policy.hpp | 2 +- .../flex-stacker/flex-stacker/errors.hpp | 1 + .../flex-stacker/flex-stacker/gcodes.hpp | 53 ++++++++++ .../flex-stacker/host_comms_task.hpp | 32 +++++- .../flex-stacker/flex-stacker/messages.hpp | 11 ++- .../flex-stacker/flex-stacker/ui_task.hpp | 99 ++++++++++--------- .../include/flex-stacker/systemwide.h | 12 +++ 10 files changed, 160 insertions(+), 73 deletions(-) diff --git a/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c b/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c index 2d0640159..82f2751f0 100644 --- a/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c +++ b/stm32-modules/flex-stacker/firmware/system/i2c_hardware.c @@ -174,15 +174,6 @@ bool i2c_register_handle(HAL_I2C_HANDLE handle, I2C_BUS bus) { return true; } -/** - * @brief Checks if the device is communicating. - */ -uint8_t hal_i2c_comms_ready(HAL_I2C_HANDLE handle, uint16_t dev_address) { - HAL_StatusTypeDef status = HAL_OK; - //status = HAL_I2C_IsDeviceReady(handle, dev_address, MAX_RETRIES, MAX_TIMEOUT); - return status; -} - /** * Wrapper around HAL_I2C_Mem_Write */ @@ -194,11 +185,6 @@ uint8_t hal_i2c_write(I2C_BUS bus, uint16_t DevAddress, uint8_t reg, uint8_t *da // Bus was not registered if(notification_handle == NULL) return false; - // Make sure the device is communicating. - HAL_StatusTypeDef dev_status = HAL_OK; - dev_status = hal_i2c_comms_ready(i2c_handle, DevAddress); - if (dev_status != HAL_OK) return dev_status; - uint8_t retries = 0; HAL_StatusTypeDef tx_result = HAL_OK; do { @@ -229,11 +215,6 @@ uint8_t hal_i2c_read(I2C_BUS bus, uint16_t DevAddress, uint16_t reg, uint8_t *da // Bus was not registered if(notification_handle == NULL) return NO_HANDLE_ERROR; - // Make sure the device is communicating. - HAL_StatusTypeDef dev_status = HAL_OK; - dev_status = hal_i2c_comms_ready(i2c_handle, DevAddress); - if (dev_status != HAL_OK) return dev_status; - uint8_t retries = 0; HAL_StatusTypeDef rx_result = HAL_OK; do { diff --git a/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp b/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp index 25762f741..7baa481f9 100644 --- a/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp +++ b/stm32-modules/flex-stacker/firmware/ui/ui_policy.cpp @@ -1,3 +1,4 @@ + #include #include "FreeRTOS.h" diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp index b1e23d767..79b0f169e 100644 --- a/stm32-modules/flex-stacker/src/errors.cpp +++ b/stm32-modules/flex-stacker/src/errors.cpp @@ -16,6 +16,8 @@ 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 SYSTEM_SET_STATUSBAR_COLOR_ERROR = + "ERR304:system:STATUSBAR communication error\n"; const char* const TMC2160_READ_ERROR = "ERR901:TMC2160 driver read error\n"; const char* const TMC2160_WRITE_ERROR = "ERR902:TMC2160 driver write error\n"; const char* const TMC2160_INVALID_ADDRESS = "ERR903:TMC2160 invalid address\n"; @@ -49,6 +51,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(SYSTEM_SET_STATUSBAR_COLOR_ERROR); HANDLE_CASE(TMC2160_READ_ERROR); HANDLE_CASE(TMC2160_WRITE_ERROR); HANDLE_CASE(TMC2160_INVALID_ADDRESS); diff --git a/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp b/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp index 6be9c26ba..5ed484082 100644 --- a/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp +++ b/stm32-modules/include/flex-stacker/firmware/ui_policy.hpp @@ -3,8 +3,8 @@ #include #include -#include "task.h" #include "i2c_comms.hpp" +#include "task.h" namespace ui_policy { using namespace i2c::hardware; diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp index f0fda3c37..8a463e8d1 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp @@ -19,6 +19,7 @@ enum class ErrorCode { SYSTEM_SERIAL_NUMBER_INVALID = 301, SYSTEM_SERIAL_NUMBER_HAL_ERROR = 302, SYSTEM_EEPROM_ERROR = 303, + SYSTEM_SET_STATUSBAR_COLOR_ERROR = 304, // 9xx - TMC2160 TMC2160_READ_ERROR = 901, TMC2160_WRITE_ERROR = 902, diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp index 21251861e..24cee59a4 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp @@ -211,4 +211,57 @@ struct GetDoorClosed { } }; +struct SetStatusBarColor { + StatusBarID bar_id; + std::optional color; + float power; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '2', '0', '0', ' '}; + static constexpr const char* response = "M200 OK\n"; + + using IArg = Arg; + using EArg = Arg; + using ColorArg = Arg; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + auto res = gcode::SingleParser::parse_gcode( + input, limit, prefix); + if (!res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + + auto ret = SetStatusBarColor{.bar_id = StatusBarID::Internal, + .color = std::nullopt, + .power = 0}; + + auto arguments = res.first.value(); + if (std::get<0>(arguments).present) { + ret.power = static_cast(std::get<0>(arguments).value); + } else if (std::get<1>(arguments).present) { + ret.bar_id = StatusBarID::External; + ret.power = static_cast(std::get<1>(arguments).value); + } else { + return std::make_pair(ParseResult(), input); + } + + if (std::get<2>(arguments).present) { + ret.color = + static_cast(std::get<2>(arguments).value); + } + return std::make_pair(ret, res.second); + } + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto write_response_into(InputIt buf, InLimit limit) -> InputIt { + return write_string_to_iterpair(buf, limit, response); + } +}; + } // namespace gcode diff --git a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp index 9f607ebd5..1c9d8e7ed 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp @@ -15,6 +15,7 @@ #include "flex-stacker/errors.hpp" #include "flex-stacker/gcodes.hpp" #include "flex-stacker/messages.hpp" +#include "gcodes.hpp" #include "gcodes_motor.hpp" #include "hal/message_queue.hpp" #include "messages.hpp" @@ -48,14 +49,15 @@ class HostCommsTask { gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams, gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor, gcode::GetPlatformSensors, gcode::GetDoorClosed, gcode::GetEstopStatus, - gcode::StopMotor>; + gcode::StopMotor, gcode::SetStatusBarColor>; using AckOnlyCache = AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber, gcode::SetTMCRegister, gcode::SetRunCurrent, gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor, gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, gcode::MoveMotorInMm, gcode::SetMicrosteps, - gcode::SetMotorStallGuard, gcode::HomeMotor, gcode::StopMotor>; + gcode::SetStatusBarColor, gcode::SetMotorStallGuard, + gcode::HomeMotor, gcode::StopMotor>; using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>; using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>; using GetLimitSwitchesCache = AckCache<8, gcode::GetLimitSwitches>; @@ -997,6 +999,32 @@ class HostCommsTask { return std::make_pair(true, tx_into); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_gcode(const gcode::SetStatusBarColor& gcode, InputIt tx_into, + InputLimit tx_limit) -> std::pair { + auto id = ack_only_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::SetStatusBarColorMessage{ + .id = id, + .bar_id = gcode.bar_id, + .power = gcode.power, + .color = gcode.color, + }; + 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); + ack_only_cache.remove_if_present(id); + return std::make_pair(false, wrote_to); + } + return std::make_pair(true, tx_into); + } + // Our error handler just writes an error and bails template requires std::forward_iterator && diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp index 9e5766905..2182d5ba0 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp @@ -272,9 +272,12 @@ struct GetEstopResponse { bool triggered; }; -// This empty message is just used to signal that the UI task should update -// its outputs -struct UpdateUIMessage {}; +struct SetStatusBarColorMessage { + uint32_t id = 0; + StatusBarID bar_id = StatusBarID::Internal; + float power = 0; + std::optional color = std::nullopt; +}; using HostCommsMessage = ::std::variant; -using UIMessage = ::std::variant; +using UIMessage = ::std::variant; using MotorDriverMessage = ::std::variant #include "core/is31fl_driver.hpp" +#include "errors.hpp" #include "hal/message_queue.hpp" #include "messages.hpp" #include "ot_utils/freertos/freertos_timer.hpp" +#include "systemwide.h" #include "ui_policy.hpp" namespace ui_task { @@ -20,8 +22,6 @@ concept UIPolicyIface = requires(Policy& p) { } &&is31fl::IS31FL_Policy; -enum Color { W, R, G, B }; - // There are 3 channels per color using ChannelMapping = std::array; @@ -30,38 +30,36 @@ static constexpr ChannelMapping red_channels{6, 9, 12}; static constexpr ChannelMapping green_channels{7, 10, 13}; static constexpr ChannelMapping blue_channels{8, 11, 14}; -static auto color_to_channels(Color color) -> const ChannelMapping& { +static auto color_to_channels(StatusBarColor color) -> const ChannelMapping& { switch (color) { - case Color::W: + case StatusBarColor::White: return white_channels; - case Color::R: + case StatusBarColor::Red: return red_channels; - case Color::G: + case StatusBarColor::Green: return green_channels; - case Color::B: + case StatusBarColor::Blue: return blue_channels; default: return white_channels; } } -enum StatusBar { Inside, Outside }; - typedef struct StatusBarState { - StatusBar kind; - Color color; + StatusBarID kind; + StatusBarColor color; float power; } StatusBarState; StatusBarState led_bar_internal = { - .kind = StatusBar::Inside, - .color = Color::G, + .kind = StatusBarID::Internal, + .color = StatusBarColor::Blue, .power = 0.5F, }; StatusBarState led_bar_external = { - .kind = StatusBar::Outside, - .color = Color::G, + .kind = StatusBarID::External, + .color = StatusBarColor::Blue, .power = 0.5F, }; @@ -110,13 +108,13 @@ class UITask { if (!_led_driver0.initialized()) { _led_driver0.initialize(policy); - StatusBarState bar_state = get_statusbar_state(Inside); - set_status_bar(Inside, bar_state.color, bar_state.power); + StatusBarState bar_state = get_statusbar_state(Internal); + set_status_bar(Internal, bar_state.color, bar_state.power); } if (!_led_driver1.initialized()) { _led_driver1.initialize(policy); - StatusBarState bar_state = get_statusbar_state(Outside); - set_status_bar(Outside, bar_state.color, bar_state.power); + StatusBarState bar_state = get_statusbar_state(External); + set_status_bar(External, bar_state.color, bar_state.power); } _initialized = true; } @@ -130,70 +128,77 @@ class UITask { private: template - auto visit_message(const std::monostate& message, Policy& policy) -> void { - static_cast(message); + auto visit_message(const std::monostate& m, Policy& policy) -> void { + static_cast(m); static_cast(policy); } template - auto visit_message(const messages::UpdateUIMessage& message, Policy& policy) - -> void { - static_cast(message); + auto visit_message(const messages::SetStatusBarColorMessage& m, + Policy& policy) -> void { + static_cast(policy); + auto response = messages::AcknowledgePrevious{.responding_to_id = m.id}; - // TODO: This should be set from the Message - set_status_bar(Inside, Color::B, 0.5F); + StatusBarState bar = get_statusbar_state(m.bar_id); + StatusBarColor color = bar.color; + if (m.color.has_value()) color = m.color.value(); + if(!set_status_bar(m.bar_id, color, m.power)) { + response.with_error = errors::ErrorCode::SYSTEM_SET_STATUSBAR_COLOR_ERROR; + } + + static_cast(_task_registry->send_to_address( + response, Queues::HostCommsAddress)); } /** * @brief Set the power (separate from PWM) for a color. Each color has * 3 channels, so this helper will set all of the channels. */ - auto set_color_power(StatusBar bar, Color color, float power) -> bool { + auto set_color_power(StatusBarID bar, StatusBarColor color, float power) -> bool { const auto& channels = color_to_channels(color); // clear the current leds - if (bar == Inside) _led_driver0.set_current(0); - if (bar == Outside) _led_driver1.set_current(0); + if (bar == Internal) _led_driver0.set_current(0); + if (bar == External) _led_driver1.set_current(0); return std::ranges::all_of( channels.cbegin(), channels.cend(), [bar, power, this](size_t c) { - if (bar == Inside) return _led_driver0.set_current(c, power); - if (bar == Outside) return _led_driver1.set_current(c, power); + if (bar == Internal) return _led_driver0.set_current(c, power); + if (bar == External) return _led_driver1.set_current(c, power); return false; }); } + // Callback function for the heartbeat led timer auto heartbeat_led() { hb_led_state = !hb_led_state; _policy->set_heartbeat_led(hb_led_state); - - // for testing - Color color = hb_led_state ? Color::G : Color::R; - set_status_bar(Inside, color, 1); } - auto get_statusbar_state(StatusBar bar) -> StatusBarState { + // Helper to get the StatusBarState given the bar id + auto get_statusbar_state(StatusBarID bar) -> StatusBarState& { switch (bar) { - case Inside: + case Internal: return _led_bar_internal; - case Outside: + case External: return _led_bar_external; default: return _led_bar_internal; } } - auto set_status_bar(StatusBar bar, Color color, float power) -> bool { - auto bar_state = get_statusbar_state(bar); - bar_state.power = std::clamp(power, 0.0F, 1.0F); - bar_state.color = color; + auto set_status_bar(StatusBarID bar, StatusBarColor color, float power) -> bool { + power = std::clamp(power, 0.0F, 1.0F); + auto status_bar = &get_statusbar_state(bar); + status_bar->color = color; + status_bar->power = power; - set_color_power(bar, bar_state.color, bar_state.power); - if (bar == Inside) { - _led_driver0.set_pwm(bar_state.power); + set_color_power(bar, color, power); + if (bar == Internal) { + _led_driver0.set_pwm(power); return _led_driver0.send_update(*_policy); - } else if (bar == Outside) { - _led_driver1.set_pwm(bar_state.power); + } else if (bar == External) { + _led_driver1.set_pwm(power); return _led_driver1.send_update(*_policy); } return false; diff --git a/stm32-modules/include/flex-stacker/systemwide.h b/stm32-modules/include/flex-stacker/systemwide.h index 7c1abeba6..249eb44f0 100644 --- a/stm32-modules/include/flex-stacker/systemwide.h +++ b/stm32-modules/include/flex-stacker/systemwide.h @@ -7,6 +7,18 @@ typedef enum MotorID { MOTOR_L, } MotorID; +typedef enum StatusBarID { + Internal, + External, +} StatusBarID; + +typedef enum StatusBarColor { + White = 0, + Red, + Green, + Blue, +} StatusBarColor; + enum MotorSelection { Z, X, L, ALL }; /* size of array for setting serial number */