diff --git a/stm32-modules/flex-stacker/firmware/main.cpp b/stm32-modules/flex-stacker/firmware/main.cpp index 72918c2ea..bb09f4f91 100644 --- a/stm32-modules/flex-stacker/firmware/main.cpp +++ b/stm32-modules/flex-stacker/firmware/main.cpp @@ -62,6 +62,7 @@ static auto system_task = extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { switch (GPIO_Pin) { case MOTOR_DIAG0_PIN: + case N_ESTOP_PIN: static_cast(aggregator.send_from_isr( messages::GPIOInterruptMessage{.pin = GPIO_Pin})); break; diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c index 3bbc9e7eb..f3cf18343 100644 --- a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c +++ b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c @@ -53,6 +53,7 @@ typedef struct motor_hardware_struct { stepper_hardware_t motor_z; stepper_hardware_t motor_l; platform_sensor_t platform_sensors; + PinConfig estop; } motor_hardware_t; @@ -92,6 +93,7 @@ static motor_hardware_t _motor_hardware = { .x_minus = {PLAT_SENSE_MINUS_PORT, PLAT_SENSE_MINUS_PIN, GPIO_PIN_SET}, .x_plus = {PLAT_SENSE_PLUS_PORT, PLAT_SENSE_PLUS_PIN, GPIO_PIN_SET}, }, + .estop = {N_ESTOP_PORT, N_ESTOP_PIN, GPIO_PIN_RESET}, }; void motor_hardware_gpio_init(void){ @@ -176,9 +178,6 @@ void motor_hardware_gpio_init(void){ init.Pin = L_N_HELD_PIN; HAL_GPIO_Init(L_N_HELD_PORT, &init); - init.Pin = ESTOP_PIN; - HAL_GPIO_Init(ESTOP_PORT, &init); - /*Configure GPIO pins : INPUTs IRQ */ init.Mode = GPIO_MODE_IT_FALLING; init.Pull = GPIO_PULLUP; @@ -187,6 +186,11 @@ void motor_hardware_gpio_init(void){ init.Pin = MOTOR_DIAG0_PIN; HAL_GPIO_Init(MOTOR_DIAG0_PORT, &init); HAL_NVIC_SetPriority(EXTI15_10_IRQn, 6, 0); + + init.Pin = N_ESTOP_PIN; + HAL_GPIO_Init(N_ESTOP_PORT, &init); + HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); } // X motor timer @@ -391,12 +395,25 @@ bool hw_read_platform_sensor(bool direction) { _motor_hardware.platform_sensors.x_minus.active_setting; } +bool hw_read_estop(void) { + return HAL_GPIO_ReadPin(_motor_hardware.estop.port, _motor_hardware.estop.pin) == + _motor_hardware.estop.active_setting; +} + void hw_set_diag0_irq(bool enable) { enable ? HAL_NVIC_EnableIRQ(EXTI15_10_IRQn) : HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); } +bool hw_is_diag0_pin(uint16_t pin) { + return pin == MOTOR_DIAG0_PIN; +} + +bool hw_is_estop_pin(uint16_t pin) { + return pin == N_ESTOP_PIN; +} + void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&_motor_hardware.motor_l.timer); diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp index fab9740f4..517841196 100644 --- a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp +++ b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp @@ -41,3 +41,16 @@ auto MotorPolicy::check_platform_sensor(bool direction) -> bool { auto MotorPolicy::set_diag0_irq(bool enable) -> void { hw_set_diag0_irq(enable); } + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto MotorPolicy::check_estop() -> bool { return hw_read_estop(); } + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto MotorPolicy::is_diag0_pin(uint16_t pin) -> bool { + return hw_is_diag0_pin(pin); +} + +// NOLINTNEXTLINE(readability-convert-member-functions-to-static) +auto MotorPolicy::is_estop_pin(uint16_t pin) -> bool { + return hw_is_estop_pin(pin); +} \ No newline at end of file diff --git a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c index 7cfdb9287..51a70ec28 100644 --- a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c +++ b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.c @@ -129,3 +129,10 @@ void EXTI15_10_IRQHandler(void) { } } +// Estop interrupt +void EXTI9_5_IRQHandler(void) +{ + if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_6)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6); + } +} \ No newline at end of file diff --git a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h index 64294d285..007684c09 100644 --- a/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h +++ b/stm32-modules/flex-stacker/firmware/system/stm32g4xx_it.h @@ -35,6 +35,7 @@ void DebugMon_Handler(void); // void SysTick_Handler(void); void RCC_IRQHandler(void); void EXTI15_10_IRQHandler(void); +void EXTI9_5_IRQHandler(void); // void DMA1_Channel1_IRQHandler(void); // void DMA1_Channel2_IRQHandler(void); diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp index 0ec2a658f..b1e23d767 100644 --- a/stm32-modules/flex-stacker/src/errors.cpp +++ b/stm32-modules/flex-stacker/src/errors.cpp @@ -9,6 +9,7 @@ const char* const UNHANDLED_GCODE = "ERR003:unhandled gcode\n"; const char* const GCODE_CACHE_FULL = "ERR004:gcode cache full\n"; const char* const BAD_MESSAGE_ACKNOWLEDGEMENT = "ERR005:bad message acknowledgement\n"; +const char* const ESTOP_TRIGGERED = "ERR006:estop triggered\n"; const char* const SYSTEM_SERIAL_NUMBER_INVALID = "ERR301:system:serial number invalid format\n"; const char* const SYSTEM_SERIAL_NUMBER_HAL_ERROR = @@ -44,6 +45,7 @@ auto errors::errorstring(ErrorCode code) -> const char* { HANDLE_CASE(UNHANDLED_GCODE); HANDLE_CASE(GCODE_CACHE_FULL); HANDLE_CASE(BAD_MESSAGE_ACKNOWLEDGEMENT); + HANDLE_CASE(ESTOP_TRIGGERED); HANDLE_CASE(SYSTEM_SERIAL_NUMBER_INVALID); HANDLE_CASE(SYSTEM_SERIAL_NUMBER_HAL_ERROR); HANDLE_CASE(SYSTEM_EEPROM_ERROR); diff --git a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h index 365d9e726..37920aa88 100644 --- a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h +++ b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h @@ -22,6 +22,9 @@ void hw_set_direction(MotorID, bool direction); bool hw_read_limit_switch(MotorID motor_id, bool direction); void hw_set_diag0_irq(bool enable); bool hw_read_platform_sensor(bool direction); +bool hw_read_estop(void); +bool hw_is_diag0_pin(uint16_t pin); +bool hw_is_estop_pin(uint16_t pin); #ifdef __cplusplus } // extern "C" @@ -93,7 +96,7 @@ bool hw_read_platform_sensor(bool direction); #define L_N_HELD_PORT (GPIOB) /**************** COMMON ********************/ -#define ESTOP_PIN (GPIO_PIN_6) -#define ESTOP_PORT (GPIOB) +#define N_ESTOP_PIN (GPIO_PIN_6) +#define N_ESTOP_PORT (GPIOB) #define MOTOR_DIAG0_PIN (GPIO_PIN_12) #define MOTOR_DIAG0_PORT (GPIOB) diff --git a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp index c541d70f6..e06d5b2ec 100644 --- a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp +++ b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp @@ -16,6 +16,9 @@ class MotorPolicy { auto check_limit_switch(MotorID motor_id, bool direction) -> bool; auto set_diag0_irq(bool enable) -> void; auto check_platform_sensor(bool direction) -> bool; + auto check_estop() -> bool; + auto is_diag0_pin(uint16_t pin) -> bool; + auto is_estop_pin(uint16_t pin) -> bool; }; } // namespace motor_policy diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp index f59d8c420..f0fda3c37 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp @@ -14,6 +14,7 @@ enum class ErrorCode { UNHANDLED_GCODE = 3, GCODE_CACHE_FULL = 4, BAD_MESSAGE_ACKNOWLEDGEMENT = 5, + ESTOP_TRIGGERED = 6, // 3xx - System General SYSTEM_SERIAL_NUMBER_INVALID = 301, SYSTEM_SERIAL_NUMBER_HAL_ERROR = 302, diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp index d3d4c74e3..744302963 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp @@ -974,4 +974,34 @@ struct GetPlatformSensors { } }; +struct GetEstopStatus { + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '1', '1', '2'}; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + auto working = prefix_matches(input, limit, prefix); + if (working == input) { + return std::make_pair(ParseResult(), input); + } + return std::make_pair(ParseResult(GetEstopStatus()), working); + } + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto write_response_into(InputIt buf, InLimit limit, int triggered) + -> InputIt { + int res = 0; + res = snprintf(&*buf, (limit - buf), "M112 %i OK", triggered); + if (res <= 0) { + return buf; + } + return buf + res; + } +}; + } // 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 1dfa64b8d..a28bd43d0 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 @@ -47,7 +47,7 @@ class HostCommsTask { gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, gcode::MoveMotorInMm, gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams, gcode::SetMotorStallGuard, gcode::GetMotorStallGuard, gcode::HomeMotor, - gcode::GetPlatformSensors, gcode::GetDoorClosed>; + gcode::GetPlatformSensors, gcode::GetDoorClosed, gcode::GetEstopStatus>; using AckOnlyCache = AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber, gcode::SetTMCRegister, gcode::SetRunCurrent, @@ -62,6 +62,7 @@ class HostCommsTask { using GetMotorStallGuardCache = AckCache<8, gcode::GetMotorStallGuard>; using GetDoorClosedCache = AckCache<8, gcode::GetDoorClosed>; using GetPlatformSensorsCache = AckCache<8, gcode::GetPlatformSensors>; + using GetEstopCache = AckCache<8, gcode::GetEstopStatus>; public: static constexpr size_t TICKS_TO_WAIT_ON_SEND = 10; @@ -84,7 +85,9 @@ class HostCommsTask { // NOLINTNEXTLINE(readability-redundant-member-init) get_door_closed_cache(), // NOLINTNEXTLINE(readability-redundant-member-init) - get_platform_sensors_cache() {} + get_platform_sensors_cache(), + // NOLINTNEXTLINE(readability-redundant-member-init) + get_estop_cache() {} HostCommsTask(const HostCommsTask& other) = delete; auto operator=(const HostCommsTask& other) -> HostCommsTask& = delete; HostCommsTask(HostCommsTask&& other) noexcept = delete; @@ -224,7 +227,8 @@ class HostCommsTask { auto visit_message(const messages::ErrorMessage& msg, InputIt tx_into, InputLimit tx_limit) -> InputIt { // stall detected, clear the message cache. - if (msg.code == errors::ErrorCode::MOTOR_STALL_DETECTED) { + if ((msg.code == errors::ErrorCode::MOTOR_STALL_DETECTED) || + (msg.code == errors::ErrorCode::ESTOP_TRIGGERED)) { ack_only_cache.clear(); } return errors::write_into_async(tx_into, tx_limit, msg.code); @@ -405,6 +409,28 @@ class HostCommsTask { cache_entry); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_message(const messages::GetEstopResponse& response, + InputIt tx_into, InputLimit tx_limit) -> InputIt { + auto cache_entry = + get_estop_cache.remove_if_present(response.responding_to_id); + return std::visit( + [tx_into, tx_limit, response](auto cache_element) { + using T = std::decay_t; + if constexpr (std::is_same_v) { + return errors::write_into( + tx_into, tx_limit, + errors::ErrorCode::BAD_MESSAGE_ACKNOWLEDGEMENT); + } else { + return cache_element.write_response_into( + tx_into, tx_limit, response.triggered); + } + }, + cache_entry); + } + template requires std::forward_iterator && std::sized_sentinel_for @@ -949,6 +975,27 @@ class HostCommsTask { return std::make_pair(true, tx_into); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_gcode(const gcode::GetEstopStatus& gcode, InputIt tx_into, + InputLimit tx_limit) -> std::pair { + auto id = get_estop_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::GetEstopMessage{.id = id}; + 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_estop_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 && @@ -971,6 +1018,7 @@ class HostCommsTask { GetMotorStallGuardCache get_motor_stall_guard_cache; GetDoorClosedCache get_door_closed_cache; GetPlatformSensorsCache get_platform_sensors_cache; + GetEstopCache get_estop_cache; bool may_connect_latch = true; }; diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp index 25dab68e2..d9e6df612 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp @@ -263,12 +263,22 @@ struct HomeMotorMessage { bool direction; }; +struct GetEstopMessage { + uint32_t id; +}; + +struct GetEstopResponse { + uint32_t responding_to_id; + bool triggered; +}; + using HostCommsMessage = ::std::variant; + GetDoorClosedResponse, GetPlatformSensorsResponse, + GetEstopResponse>; using SystemMessage = ::std::variant; + HomeMotorMessage, GetPlatformSensorsMessage, GetEstopMessage>; }; // namespace messages diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp index 092ef629e..83729b20e 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp @@ -348,6 +348,15 @@ class MotorTask { response, Queues::HostCommsAddress)); } + template + auto visit_message(const messages::GetEstopMessage& m, Policy& policy) + -> void { + auto response = messages::GetEstopResponse{ + .responding_to_id = m.id, .triggered = policy.check_estop()}; + static_cast(_task_registry->send_to_address( + response, Queues::HostCommsAddress)); + } + template auto visit_message(const messages::MoveCompleteMessage& m, Policy& policy) -> void { @@ -400,11 +409,17 @@ class MotorTask { auto visit_message(const messages::GPIOInterruptMessage& m, Policy& policy) -> void { static_cast(m); - static_cast(policy); + if (policy.is_diag0_pin(m.pin)) { + send_error_message(Error::MOTOR_STALL_DETECTED); + } else if (policy.is_estop_pin(m.pin)) { + send_error_message(Error::ESTOP_TRIGGERED); + } else { + // don't care about other interrupts + return; + } _z_controller.stop_movement(0, true); _x_controller.stop_movement(0, false); _l_controller.stop_movement(0, false); - send_error_message(Error::MOTOR_STALL_DETECTED); } /**