From bfa0b85b1154998ef653f2697ae5b8cec1106976 Mon Sep 17 00:00:00 2001 From: pmoegenburg Date: Tue, 31 Oct 2023 21:59:14 -0400 Subject: [PATCH] PR review changes part 2 --- include/can/core/ids.hpp | 1 + include/can/core/message_handlers/motion.hpp | 3 +- .../brushed_motion_controller.hpp | 8 ++- .../brushed_motor_interrupt_handler.hpp | 2 +- .../core/motor_hardware_interface.hpp | 11 +++- .../core/stepper_motor/motion_controller.hpp | 20 ++++--- .../stepper_motor/motor_interrupt_handler.hpp | 17 +++--- .../core/stepper_motor/tmc2130_driver.hpp | 4 +- .../core/stepper_motor/tmc2160_driver.hpp | 4 +- .../tasks/gear_tmc2160_motor_driver_task.hpp | 6 +- .../core/tasks/motion_controller_task.hpp | 15 ++--- .../core/tasks/tmc2130_motor_driver_task.hpp | 8 ++- .../core/tasks/tmc2160_motor_driver_task.hpp | 6 +- .../brushed_motor/brushed_motor_hardware.hpp | 20 +++++-- .../firmware/stepper_motor/motor_hardware.hpp | 22 ++++--- .../simulation/sim_motor_hardware_iface.hpp | 58 ++++++++++++++----- .../tests/mock_brushed_motor_components.hpp | 20 ++++--- .../tests/mock_motor_hardware.hpp | 20 ++++--- .../test_brushed_motor_interrupt_handler.cpp | 3 +- .../tests/test_motor_interrupt_handler.cpp | 3 +- 20 files changed, 163 insertions(+), 88 deletions(-) diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index a3b6764fa..8c5c0d338 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -160,6 +160,7 @@ enum class ErrorCode { /** Error Severity levels. */ enum class ErrorSeverity { + none = 0x0, warning = 0x1, recoverable = 0x2, unrecoverable = 0x3, diff --git a/include/can/core/message_handlers/motion.hpp b/include/can/core/message_handlers/motion.hpp index e360f91ec..f9e23cb80 100644 --- a/include/can/core/message_handlers/motion.hpp +++ b/include/can/core/message_handlers/motion.hpp @@ -18,7 +18,8 @@ class MotionHandler { GetMotionConstraintsRequest, SetMotionConstraints, ReadLimitSwitchRequest, MotorPositionRequest, UpdateMotorPositionEstimationRequest, GetMotorUsageRequest, - MotorDriverErrorEncountered, ResetMotorDriverErrorHandling>; + MotorDriverErrorEncountered, + ResetMotorDriverErrorHandling>; MotionHandler(MotionTaskClient &motion_client) : motion_client{motion_client} {} diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp index 0c57df98f..379d74b6d 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -101,13 +101,15 @@ class MotionController { enabled = false; } - void stop(can::ids::ErrorSeverity error_severity = - can::ids::ErrorSeverity::warning) { + void stop( + can::ids::ErrorSeverity error_severity = + can::ids::ErrorSeverity::warning, + can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) { queue.reset(); // if we're gripping something we need to flag this so we don't drop it if (!hardware.get_stay_enabled()) { if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(static_cast(error_severity)); + hardware.request_cancel(error_severity, error_code); } } } diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp index 798010913..06b02100f 100644 --- a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp @@ -201,7 +201,7 @@ class BrushedMotorInterruptHandler { } else if (estop_triggered()) { in_estop = true; cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); - } else if (hardware.has_cancel_request() != 0U) { + } else if (hardware.has_cancel_request().code != 0U) { if (!hardware.get_stay_enabled()) { hardware.set_motor_state(BrushedMotorState::UNHOMED); } diff --git a/include/motor-control/core/motor_hardware_interface.hpp b/include/motor-control/core/motor_hardware_interface.hpp index f375cd68c..b888df384 100644 --- a/include/motor-control/core/motor_hardware_interface.hpp +++ b/include/motor-control/core/motor_hardware_interface.hpp @@ -72,6 +72,12 @@ class UsageEEpromConfig { size_t num_keys = 0; }; +// std::optional usage? See HardwareConfig struct +struct __attribute__((packed)) CancelRequest { + uint8_t severity; + uint8_t code; +}; + class MotorHardwareIface { public: MotorHardwareIface() = default; @@ -98,8 +104,9 @@ class MotorHardwareIface { virtual void enable_encoder() = 0; virtual void disable_encoder() = 0; - virtual auto has_cancel_request() -> uint8_t = 0; - virtual void request_cancel(uint8_t error_severity) = 0; + virtual auto has_cancel_request() -> CancelRequest = 0; + virtual void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) = 0; virtual void clear_cancel_request() = 0; virtual auto get_usage_eeprom_config() -> const UsageEEpromConfig& = 0; diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index a5021f3be..4d990317e 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -107,17 +107,17 @@ class MotionController { return update_queue.try_write(can_msg); } - void stop(can::ids::ErrorSeverity error_severity = - can::ids::ErrorSeverity::warning) { + void stop( + can::ids::ErrorSeverity error_severity = + can::ids::ErrorSeverity::warning, + can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) { queue.reset(); if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(static_cast(error_severity)); + hardware.request_cancel(error_severity, error_code); } } - void clear_cancel_request() { - hardware.clear_cancel_request(); - } + void clear_cancel_request() { hardware.clear_cancel_request(); } auto is_timer_interrupt_running() -> bool { return hardware.is_timer_interrupt_running(); @@ -268,14 +268,16 @@ class PipetteMotionController { return false; } - void stop(can::ids::ErrorSeverity error_severity = - can::ids::ErrorSeverity::warning) { + void stop( + can::ids::ErrorSeverity error_severity = + can::ids::ErrorSeverity::warning, + can::ids::ErrorCode error_code = can::ids::ErrorCode::stop_requested) { queue.reset(); // if the timer interrupt is running, cancel it. if it isn't running, // don't submit a cancel because then the cancel won't be read until // the timer starts the next time. if (hardware.is_timer_interrupt_running()) { - hardware.request_cancel(static_cast(error_severity)); + hardware.request_cancel(error_severity, error_code); } } diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp index 6b3829f70..1d5bcdaca 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -187,7 +187,8 @@ class MotorInterruptHandler { void run_interrupt() { // handle various error states - uint8_t has_cancel_request = hardware.has_cancel_request(); + motor_hardware::CancelRequest cancel_request = + hardware.has_cancel_request(); if (clear_queue_until_empty) { // If we were executing a move when estop asserted, and // what's in the queue is the remaining enqueued moves from @@ -206,14 +207,12 @@ class MotorInterruptHandler { } else if (estop_triggered()) { cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); in_estop = true; - } else if (has_cancel_request != 0U) { - if (has_cancel_request == - static_cast(can::ids::ErrorSeverity::unrecoverable)) { - cancel_and_clear_moves(can::ids::ErrorCode::motor_driver_error_detected); // use cancel_request.code? - } else { - cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, - can::ids::ErrorSeverity::warning); - } + } else if (cancel_request.code != + 0U) { // is this correct? Should be zero-initialized. Check, + // confirm! + cancel_and_clear_moves( + static_cast(cancel_request.code), + static_cast(cancel_request.severity)); } else { // Normal Move logic run_normal_interrupt(); diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp index 887f9cd02..7c3a74e7d 100644 --- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp +++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp @@ -52,8 +52,8 @@ class TMC2130 { auto operator=(const TMC2130&& c) = delete; ~TMC2130() = default; - auto read(Registers addr, uint32_t command_data, uint32_t message_index, uint8_t tags = 0) - -> void { + auto read(Registers addr, uint32_t command_data, uint32_t message_index, + uint8_t tags = 0) -> void { auto converted_addr = static_cast(addr); uint32_t token = spi::utils::build_token(converted_addr, tags); _spi_manager.read(token, command_data, _task_queue, _cs_intf, diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp index 99b991345..63f9ad20e 100644 --- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp +++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp @@ -53,8 +53,8 @@ class TMC2160 { auto operator=(const TMC2160&& c) = delete; ~TMC2160() = default; - auto read(Registers addr, uint32_t command_data, uint32_t message_index, uint8_t tags = 0) - -> void { + auto read(Registers addr, uint32_t command_data, uint32_t message_index, + uint8_t tags = 0) -> void { auto converted_addr = static_cast(addr); uint32_t token = spi::utils::build_token(converted_addr, tags); _spi_manager.read(token, command_data, _task_queue, _cs_intf, diff --git a/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp index 966925a3e..1760a93c4 100644 --- a/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/gear_tmc2160_motor_driver_task.hpp @@ -82,7 +82,8 @@ class MotorDriverMessageHandler { LOG("Received read motor driver request: addr=%d", m.reg_address); uint32_t data = 0; if (tmc2160::registers::is_valid_address(m.reg_address)) { - driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index); + driver.read(tmc2160::registers::Registers(m.reg_address), data, + m.message_index); } } @@ -91,7 +92,8 @@ class MotorDriverMessageHandler { uint32_t data = 0; std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE}; uint8_t tag_byte = spi::utils::byte_from_tags(tags); - driver.read(tmc2160::registers::Registers::DRVSTATUS, data, m.message_index, tag_byte); + driver.read(tmc2160::registers::Registers::DRVSTATUS, data, + m.message_index, tag_byte); } void handle(const can::messages::GearWriteMotorCurrentRequest& m) { diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index 08af6c3fe..adaf15c82 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -147,17 +147,19 @@ class MotionControllerMessageHandler { void handle(const can::messages::MotorDriverErrorEncountered& m) { if (!driver_error_handled()) { // check if gpio is low before making controller calls - controller.stop(can::ids::ErrorSeverity::unrecoverable); + controller.stop(can::ids::ErrorSeverity::unrecoverable, + can::ids::ErrorCode::motor_driver_error_detected); if (!controller.is_timer_interrupt_running()) { can_client.send_can_message( can::ids::NodeId::host, can::messages::ErrorMessage{ .message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, - .error_code = can::ids::ErrorCode::motor_driver_error_detected}); + .error_code = + can::ids::ErrorCode::motor_driver_error_detected}); driver_client.send_motor_driver_queue( - can::messages::ReadMotorDriverErrorStatus{.message_index = - m.message_index}); + can::messages::ReadMotorDriverErrorStatus{ + .message_index = m.message_index}); } } } @@ -225,9 +227,8 @@ class MotionControllerTask { // also create top level query msg void run_diag0_interrupt() { - static_cast( - queue.try_write_isr(can::messages::MotorDriverErrorEncountered{ - .message_index = 0})); + static_cast(queue.try_write_isr( + can::messages::MotorDriverErrorEncountered{.message_index = 0})); } private: diff --git a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp index 3821d5f1d..764356033 100644 --- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp @@ -81,16 +81,18 @@ class MotorDriverMessageHandler { LOG("Received read motor driver request: addr=%d", m.reg_address); uint32_t data = 0; if (tmc2130::registers::is_valid_address(m.reg_address)) { - driver.read(tmc2130::registers::Registers(m.reg_address), data, m.message_index); + driver.read(tmc2130::registers::Registers(m.reg_address), data, + m.message_index); } } void handle(const can::messages::ReadMotorDriverErrorStatus& m) { LOG("Received read motor driver error register request"); - uint32_t data = 0; + uint32_t data = 0; std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE}; uint8_t tag_byte = spi::utils::byte_from_tags(tags); - driver.read(tmc2130::registers::Registers::DRVSTATUS, data, m.message_index, tag_byte); + driver.read(tmc2130::registers::Registers::DRVSTATUS, data, + m.message_index, tag_byte); } void handle(const can::messages::WriteMotorCurrentRequest& m) { diff --git a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp index 9030248b6..568c7d4c6 100644 --- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp @@ -94,7 +94,8 @@ class MotorDriverMessageHandler { LOG("Received read motor driver request: addr=%d", m.reg_address); uint32_t data = 0; if (tmc2160::registers::is_valid_address(m.reg_address)) { - driver.read(tmc2160::registers::Registers(m.reg_address), data, m.message_index); + driver.read(tmc2160::registers::Registers(m.reg_address), data, + m.message_index); } } @@ -103,7 +104,8 @@ class MotorDriverMessageHandler { uint32_t data = 0; std::array tags{spi::utils::ResponseTag::IS_ERROR_RESPONSE}; uint8_t tag_byte = spi::utils::byte_from_tags(tags); - driver.read(tmc2160::registers::Registers::DRVSTATUS, data, m.message_index, tag_byte); + driver.read(tmc2160::registers::Registers::DRVSTATUS, data, + m.message_index, tag_byte); } void handle(const can::messages::WriteMotorCurrentRequest& m) { diff --git a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp index 109642127..c42b2dba8 100644 --- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp +++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp @@ -45,6 +45,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { controller_loop{config.pid_kp, config.pid_ki, config.pid_kd, 1.F / config.encoder_interrupt_freq, config.wl_high, config.wl_low}, + cancel_request(), eeprom_config{eeprom_config}, stopwatch_handle{stopwatch_handle} {} BrushedMotorHardware(const BrushedMotorHardware&) = delete; @@ -78,14 +79,21 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { void reset_control() final; void set_stay_enabled(bool state) final { stay_enabled = state; } auto get_stay_enabled() -> bool final { return stay_enabled; } - auto has_cancel_request() -> uint8_t final { - return cancel_request.exchange(0); + // need any std::optional usage? + auto has_cancel_request() -> CancelRequest final { + CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); } - void request_cancel(uint8_t error_severity) final { - cancel_request.store(error_severity); + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request.store(update_request); } void clear_cancel_request() final { - cancel_request.store(0); + CancelRequest clear_request; + cancel_request.store(clear_request); } auto get_usage_eeprom_config() -> const UsageEEpromConfig& final { return eeprom_config; @@ -106,7 +114,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { int32_t motor_encoder_overflow_count = 0; ot_utils::pid::PID controller_loop; std::atomic control_dir = ControlDirection::unset; - std::atomic cancel_request = 0; + std::atomic cancel_request; const UsageEEpromConfig& eeprom_config; void* stopwatch_handle; BrushedMotorState motor_state = BrushedMotorState::UNHOMED; diff --git a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp index 00e31933c..4846d3400 100644 --- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp +++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp @@ -31,7 +31,8 @@ class MotorHardware : public StepperMotorHardwareIface { : pins(config), tim_handle(timer_handle), enc_handle(encoder_handle), - eeprom_config(eeprom_config) {} + eeprom_config(eeprom_config), + cancel_request() {} MotorHardware(const MotorHardware&) = delete; auto operator=(const MotorHardware&) -> MotorHardware& = delete; MotorHardware(MotorHardware&&) = delete; @@ -57,14 +58,21 @@ class MotorHardware : public StepperMotorHardwareIface { void disable_encoder() final; void enable_encoder() final; - auto has_cancel_request() -> uint8_t final { - return cancel_request.exchange(0); + // need any std::optional usage? + auto has_cancel_request() -> CancelRequest final { + CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); } - void request_cancel(uint8_t error_severity) final { - cancel_request.store(error_severity); + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request.store(update_request); } void clear_cancel_request() final { - cancel_request.store(0); + CancelRequest clear_request; + cancel_request.store(clear_request); } auto get_usage_eeprom_config() -> const UsageEEpromConfig& final { @@ -83,7 +91,7 @@ class MotorHardware : public StepperMotorHardwareIface { void* enc_handle; const UsageEEpromConfig& eeprom_config; std::atomic motor_encoder_overflow_count = 0; - std::atomic cancel_request = 0; + std::atomic cancel_request; static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31; }; diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp index c858d6fa3..fd244af27 100644 --- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp +++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp @@ -87,11 +87,20 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface { bool check_estop_in() final { return estop_detected; } void set_estop(bool estop_pressed) { estop_detected = estop_pressed; } - auto has_cancel_request() -> uint8_t final { - return cancel_request.exchange(0); + auto has_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); } - void request_cancel(uint8_t error_severity) final { - cancel_request.store(error_severity); + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request; + cancel_request.store(clear_request); } auto get_usage_eeprom_config() -> motor_hardware::UsageEEpromConfig & final { @@ -108,7 +117,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface { Direction _direction = Direction::POSITIVE; float _encoder_ticks_per_pulse = 0; bool estop_detected = false; - std::atomic cancel_request = 0; + std::atomic cancel_request = {}; motor_hardware::UsageEEpromConfig eeprom_config = { std::array{UsageRequestSet{ .eeprom_key = 0, @@ -179,13 +188,21 @@ class SimBrushedMotorHardwareIface return ret; } - auto has_cancel_request() -> uint8_t final { - return cancel_request.exchange(0); + auto has_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); } - void request_cancel(uint8_t error_severity) final { - cancel_request.store(error_severity); + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request; + cancel_request.store(clear_request); } - void disable_encoder() final {} void enable_encoder() final {} @@ -205,7 +222,7 @@ class SimBrushedMotorHardwareIface StateManagerHandle _state_manager = nullptr; MoveMessageHardware _id; bool estop_detected = false; - std::atomic cancel_request = 0; + std::atomic cancel_request = {}; BrushedMotorState motor_state = BrushedMotorState::UNHOMED; motor_hardware::UsageEEpromConfig eeprom_config{ std::array{UsageRequestSet{ @@ -271,11 +288,20 @@ class SimGearMotorHardwareIface bool check_estop_in() final { return estop_detected; } void set_estop(bool estop_pressed) { estop_detected = estop_pressed; } - auto has_cancel_request() -> uint8_t final { - return cancel_request.exchange(0); + auto has_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); + } + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request.store(update_request); } - void request_cancel(uint8_t error_severity) final { - cancel_request.store(error_severity); + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request; + cancel_request.store(clear_request); } auto get_usage_eeprom_config() @@ -293,7 +319,7 @@ class SimGearMotorHardwareIface Direction _direction = Direction::POSITIVE; float _encoder_ticks_per_pulse = 0; bool estop_detected = false; - std::atomic cancel_request = 0; + std::atomic cancel_request = {}; motor_hardware::UsageEEpromConfig eeprom_config = { std::array{UsageRequestSet{ .eeprom_key = 0, diff --git a/include/motor-control/tests/mock_brushed_motor_components.hpp b/include/motor-control/tests/mock_brushed_motor_components.hpp index e0c467061..95b95d045 100644 --- a/include/motor-control/tests/mock_brushed_motor_components.hpp +++ b/include/motor-control/tests/mock_brushed_motor_components.hpp @@ -71,16 +71,22 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { PWM_DIRECTION get_direction() { return move_dir; } void set_stay_enabled(bool state) { stay_enabled = state; } auto get_stay_enabled() -> bool { return stay_enabled; } - auto has_cancel_request() -> uint8_t final { - uint8_t old_request = cancel_request; - cancel_request = 0; + auto has_cancel_request() -> CancelRequest final { + CancelRequest old_request = cancel_request; + CancelRequest exchange_request{}; + cancel_request = exchange_request; return old_request; } - void request_cancel(uint8_t error_severity) final { - cancel_request = error_severity; + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request = update_request; } void clear_cancel_request() final { - cancel_request = 0; + CancelRequest clear_request{}; + cancel_request = clear_request; } void set_timer_interrupt_running(bool is_running) { timer_interrupt_running = is_running; @@ -113,7 +119,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { // when the "motor" instantly goes to top speed then instantly stops ot_utils::pid::PID controller_loop{0.008, 0.0045, 0.000015, 1.F / 32000.0, 7, -7}; - uint8_t cancel_request = 0; + CancelRequest cancel_request = {}; bool timer_interrupt_running = true; motor_hardware::UsageEEpromConfig eeprom_config = motor_hardware::UsageEEpromConfig{std::array{ diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp index 6c681e6cd..96d0fb42d 100644 --- a/include/motor-control/tests/mock_motor_hardware.hpp +++ b/include/motor-control/tests/mock_motor_hardware.hpp @@ -39,16 +39,22 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface { void reset_encoder_pulses() final { test_pulses = 0; } int32_t get_encoder_pulses() final { return test_pulses; } void sim_set_encoder_pulses(int32_t pulses) { test_pulses = pulses; } - auto has_cancel_request() -> uint8_t final { - uint8_t old_request = cancel_request; - cancel_request = 0; + auto has_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest old_request = cancel_request; + motor_hardware::CancelRequest exchange_request{}; + cancel_request = exchange_request; return old_request; } - void request_cancel(uint8_t error_severity) final { - cancel_request = error_severity; + void request_cancel(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast(error_severity), + .code = static_cast(error_code)}; + cancel_request = update_request; } void clear_cancel_request() final { - cancel_request = 0; + motor_hardware::CancelRequest clear_request{}; + cancel_request = clear_request; } void sim_set_timer_interrupt_running(bool is_running) { mock_timer_interrupt_running = is_running; @@ -70,7 +76,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface { bool mock_dir_value = false; uint8_t finished_move_id = 0x0; int32_t test_pulses = 0x0; - uint8_t cancel_request = 0; + motor_hardware::CancelRequest cancel_request = {}; bool mock_timer_interrupt_running = true; motor_hardware::UsageEEpromConfig eeprom_config = motor_hardware::UsageEEpromConfig{ diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index a8f08c1d9..767a85ce8 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -512,7 +512,8 @@ SCENARIO("handler recovers from error state") { test_objs.handler.error_handled = true; WHEN("a cancel request is received") { - test_objs.hw.request_cancel(1); + test_objs.hw.request_cancel(can::ids::ErrorSeverity::warning, + can::ids::ErrorCode::stop_requested); test_objs.handler.run_interrupt(); THEN( "motor state should become un-homed only if stay engaged is " diff --git a/motor-control/tests/test_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp index b0f2bd75c..5cd8d135d 100644 --- a/motor-control/tests/test_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_motor_interrupt_handler.cpp @@ -44,7 +44,8 @@ SCENARIO("a move is cancelled due to a stop request") { test_objs.handler.run_interrupt(); test_objs.handler.run_interrupt(); CHECK(test_objs.hw.steps_taken() == 1); - test_objs.hw.request_cancel(1); + test_objs.hw.request_cancel(can::ids::ErrorSeverity::warning, + can::ids::ErrorCode::stop_requested); test_objs.handler.run_interrupt(); THEN("An error and increase error count is sent") { REQUIRE(test_objs.reporter.messages.size() == 2);