diff --git a/.github/workflows/build-tests.yaml b/.github/workflows/build-tests.yaml index b6fbfd09d..375c02434 100644 --- a/.github/workflows/build-tests.yaml +++ b/.github/workflows/build-tests.yaml @@ -18,7 +18,7 @@ jobs: build-and-test: name: Run all tests runs-on: "ubuntu-20.04" - timeout-minutes: 10 + timeout-minutes: 20 steps: - name: Checkout ot3-firmware repo uses: actions/checkout@v4 diff --git a/gantry/core/tasks_proto.cpp b/gantry/core/tasks_proto.cpp index de020ffc3..965adbd0f 100644 --- a/gantry/core/tasks_proto.cpp +++ b/gantry/core/tasks_proto.cpp @@ -55,18 +55,20 @@ static auto eeprom_data_rev_update_builder = /** * Start gantry tasks. */ -void gantry::tasks::start_tasks( +auto gantry::tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::BeltConfig>& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler { auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); - auto& motion = mc_task_builder.start(5, "motion controller", - motion_controller, ::queues, ::queues); + auto& motion = + mc_task_builder.start(5, "motion controller", motion_controller, + ::queues, ::queues, ::queues, ::queues); auto& tmc2130_driver = motor_driver_task_builder.start( 5, "tmc2130 driver", driver_configs, ::queues, spi_task_client); auto& move_group = @@ -116,6 +118,15 @@ void gantry::tasks::start_tasks( ::queues.usage_storage_queue = &usage_storage_task.get_queue(); mh_tsk.start_task(); + + return gantry::tasks::call_run_diag0_interrupt; +} + +void gantry::tasks::call_run_diag0_interrupt() { + if (gantry::tasks::get_tasks().motion_controller) { + return gantry::tasks::get_tasks() + .motion_controller->run_diag0_interrupt(); + } } gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw) @@ -131,6 +142,11 @@ void gantry::queues::QueueClient::send_motor_driver_queue( motor_driver_queue->try_write(m); } +void gantry::queues::QueueClient::send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) { + static_cast<void>(motor_driver_queue->try_write_isr(m)); +} + void gantry::queues::QueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/gantry/core/tasks_rev1.cpp b/gantry/core/tasks_rev1.cpp index 95c81ba29..4ba6f156c 100644 --- a/gantry/core/tasks_rev1.cpp +++ b/gantry/core/tasks_rev1.cpp @@ -54,18 +54,20 @@ static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{queues}; /** * Start gantry ::tasks. */ -void gantry::tasks::start_tasks( +auto gantry::tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::BeltConfig>& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2160::configs::TMC2160DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler { auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); - auto& motion = mc_task_builder.start(5, "motion controller", - motion_controller, ::queues, ::queues); + auto& motion = + mc_task_builder.start(5, "motion controller", motion_controller, + ::queues, ::queues, ::queues, ::queues); auto& tmc2160_driver = motor_driver_task_builder.start( 5, "tmc2160 driver", driver_configs, ::queues, spi_task_client); auto& move_group = @@ -115,6 +117,15 @@ void gantry::tasks::start_tasks( ::queues.usage_storage_queue = &usage_storage_task.get_queue(); mh_tsk.start_task(); + + return gantry::tasks::call_run_diag0_interrupt; +} + +void gantry::tasks::call_run_diag0_interrupt() { + if (gantry::tasks::get_tasks().motion_controller) { + return gantry::tasks::get_tasks() + .motion_controller->run_diag0_interrupt(); + } } gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw) @@ -130,6 +141,11 @@ void gantry::queues::QueueClient::send_motor_driver_queue( motor_driver_queue->try_write(m); } +void gantry::queues::QueueClient::send_motor_driver_queue_isr( + const tmc2160::tasks::TaskMessage& m) { + static_cast<void>(motor_driver_queue->try_write_isr(m)); +} + void gantry::queues::QueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/gantry/firmware/interfaces_proto.cpp b/gantry/firmware/interfaces_proto.cpp index 584fd990e..0c142d197 100644 --- a/gantry/firmware/interfaces_proto.cpp +++ b/gantry/firmware/interfaces_proto.cpp @@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x { .port = GPIOB, .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; @@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y { .port = GPIOB, .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0x2, .run_current = 0x18, .hold_current_delay = 0x7}, @@ -156,7 +168,7 @@ static tmc2130::configs::TMC2130DriverConfig gantry_x_driver_configs{ }}; static tmc2130::configs::TMC2130DriverConfig gantry_y_driver_configs{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0x2, .run_current = 0x18, .hold_current_delay = 0x7}, @@ -232,8 +244,8 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck, - update_position_queue); + motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), + motor_hardware_iface, stallcheck, update_position_queue); static auto encoder_background_timer = motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface); @@ -269,13 +281,14 @@ static constexpr auto can_bit_timings = can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100, 500 * can::bit_timings::KHZ, 800>{}; -void interfaces::initialize() { +void interfaces::initialize(diag0_handler* call_diag0_handler) { // Initialize SPI if (initialize_spi(get_axis_type()) != HAL_OK) { Error_Handler(); } - initialize_timer(call_motor_handler, enc_overflow_callback); + initialize_timer(call_motor_handler, call_diag0_handler, + enc_overflow_callback); // Start the can bus canbus.start(can_bit_timings); diff --git a/gantry/firmware/interfaces_rev1.cpp b/gantry/firmware/interfaces_rev1.cpp index bf0de3359..d34b82c10 100644 --- a/gantry/firmware/interfaces_rev1.cpp +++ b/gantry/firmware/interfaces_rev1.cpp @@ -86,10 +86,16 @@ struct motion_controller::HardwareConfig motor_pins_x { .port = GPIOB, .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; @@ -125,15 +131,21 @@ struct motion_controller::HardwareConfig motor_pins_y { .port = GPIOB, .pin = GPIO_PIN_7, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOA, + .pin = GPIO_PIN_10, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOA, - .pin = GPIO_PIN_10, + .port = GPIOC, + .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET} }; static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -159,6 +171,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{ .freewheel = 0, .pwm_reg = 0x7, .pwm_lim = 0xC}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xA7}}, .current_config = { @@ -172,7 +185,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_x{ }}; static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -198,6 +211,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_config_y{ .freewheel = 0, .pwm_reg = 0x7, .pwm_lim = 0xC}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xA7}}, .current_config = { @@ -257,8 +271,8 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck, - update_position_queue); + motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), + motor_hardware_iface, stallcheck, update_position_queue); static auto encoder_background_timer = motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface); @@ -295,13 +309,14 @@ static constexpr auto can_bit_timings = can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100, 500 * can::bit_timings::KHZ, 800>{}; -void interfaces::initialize() { +void interfaces::initialize(diag0_handler* call_diag0_handler) { // Initialize SPI if (initialize_spi(get_axis_type()) != HAL_OK) { Error_Handler(); } - initialize_timer(call_motor_handler, enc_overflow_callback); + initialize_timer(call_motor_handler, call_diag0_handler, + enc_overflow_callback); // Start the can bus canbus.start(can_bit_timings); diff --git a/gantry/firmware/main_proto.cpp b/gantry/firmware/main_proto.cpp index cf7c26e03..55d10cc16 100644 --- a/gantry/firmware/main_proto.cpp +++ b/gantry/firmware/main_proto.cpp @@ -16,6 +16,8 @@ #include "gantry/core/tasks_proto.hpp" #include "i2c/firmware/i2c_comms.hpp" +static interfaces::diag0_handler call_diag0_handler = nullptr; + static auto i2c_comms2 = i2c::hardware::I2C(); static auto i2c_handles = I2CHandlerStruct{}; @@ -44,11 +46,11 @@ auto main() -> int { app_update_clear_flags(); - interfaces::initialize(); + interfaces::initialize(&call_diag0_handler); i2c_setup(&i2c_handles); i2c_comms2.set_handle(i2c_handles.i2c2); - gantry::tasks::start_tasks( + call_diag0_handler = gantry::tasks::start_tasks( interfaces::get_can_bus(), interfaces::get_motor().motion_controller, interfaces::get_spi(), interfaces::get_driver_config(), interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface); diff --git a/gantry/firmware/main_rev1.cpp b/gantry/firmware/main_rev1.cpp index 39fed3b73..77b198539 100644 --- a/gantry/firmware/main_rev1.cpp +++ b/gantry/firmware/main_rev1.cpp @@ -16,6 +16,8 @@ #include "gantry/core/tasks_rev1.hpp" #include "i2c/firmware/i2c_comms.hpp" +static interfaces::diag0_handler call_diag0_handler = nullptr; + static auto i2c_comms2 = i2c::hardware::I2C(); static auto i2c_handles = I2CHandlerStruct{}; @@ -44,11 +46,11 @@ auto main() -> int { app_update_clear_flags(); - interfaces::initialize(); + interfaces::initialize(&call_diag0_handler); i2c_setup(&i2c_handles); i2c_comms2.set_handle(i2c_handles.i2c2); - gantry::tasks::start_tasks( + call_diag0_handler = gantry::tasks::start_tasks( interfaces::get_can_bus(), interfaces::get_motor().motion_controller, interfaces::get_spi(), interfaces::get_driver_config(), interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface); diff --git a/gantry/firmware/motor_hardware.c b/gantry/firmware/motor_hardware.c index f48bcb6d7..637087c76 100644 --- a/gantry/firmware/motor_hardware.c +++ b/gantry/firmware/motor_hardware.c @@ -23,6 +23,8 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) { PC8 ------> Motor Step Pin Enable PA9 ------> Motor Enable Pin + DIAG0 + PC5 ------> Motor Diag0 Pin */ GPIO_InitStruct.Pin = GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -54,6 +56,12 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(GPIOA, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + // Diag0 + GPIO_InitStruct.Pin = GPIO_PIN_5; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } } SPI_HandleTypeDef hspi2 = { @@ -87,7 +95,7 @@ void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi) { HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15 | GPIO_PIN_1); HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9); - HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8); + HAL_GPIO_DeInit(GPIOC, GPIO_PIN_8 | GPIO_PIN_5); } } @@ -119,6 +127,7 @@ HAL_StatusTypeDef initialize_spi(enum GantryAxisType gantry_type) { static motor_interrupt_callback timer_callback = NULL; static encoder_overflow_callback enc_overflow_callback = NULL; +static diag0_interrupt_callback* diag0_callback = NULL; /** @@ -140,6 +149,9 @@ void MX_GPIO_Init(void) { GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(EXTI9_5_IRQn); } // motor timer: 200kHz from @@ -236,6 +248,16 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { } } +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + if (GPIO_Pin == GPIO_PIN_5) { + if (diag0_callback != NULL) { + if (*diag0_callback != NULL) { + (*diag0_callback)(); + } + } + } +} + void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) { if (htim == &htim2) { /* Peripheral clock enable */ @@ -255,9 +277,10 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim) { } } -void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback) { +void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback) { timer_callback = callback; enc_overflow_callback = enc_callback; + diag0_callback = diag0_int_callback; MX_GPIO_Init(); MX_TIM7_Init(); Encoder_GPIO_Init(); diff --git a/gantry/firmware/motor_hardware.h b/gantry/firmware/motor_hardware.h index fa2e0ac71..3a3024ac6 100644 --- a/gantry/firmware/motor_hardware.h +++ b/gantry/firmware/motor_hardware.h @@ -14,11 +14,12 @@ extern TIM_HandleTypeDef htim2; typedef void (*motor_interrupt_callback)(); typedef void (*encoder_overflow_callback)(int32_t); +typedef void (*diag0_interrupt_callback)(); HAL_StatusTypeDef initialize_spi(enum GantryAxisType); void gantry_driver_CLK_init(enum GantryAxisType); -void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback); +void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback); #ifdef __cplusplus } // extern "C" diff --git a/gantry/firmware/stm32g4xx_it.c b/gantry/firmware/stm32g4xx_it.c index b4f589e8f..07463106c 100644 --- a/gantry/firmware/stm32g4xx_it.c +++ b/gantry/firmware/stm32g4xx_it.c @@ -147,6 +147,7 @@ void FDCAN1_IT0_IRQHandler(void) { */ void TIM7_IRQHandler(void) { HAL_TIM_IRQHandler(&htim7); } void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); } +void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); } extern void xPortSysTickHandler(void); diff --git a/gantry/simulator/interfaces.cpp b/gantry/simulator/interfaces.cpp index f1a59f0bd..a22db5105 100644 --- a/gantry/simulator/interfaces.cpp +++ b/gantry/simulator/interfaces.cpp @@ -89,14 +89,16 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gantry::queues::get_queues(), motor_interface, stallcheck, - update_position_queue); + motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(), + motor_interface, stallcheck, update_position_queue); static motor_interrupt_driver::MotorInterruptDriver A(motor_queue, motor_interrupt, motor_interface, update_position_queue); -void interfaces::initialize() {} +void interfaces::initialize(diag0_handler* call_diag0_handler) { + static_cast<void>(call_diag0_handler); +} static po::variables_map options{}; diff --git a/gantry/simulator/main.cpp b/gantry/simulator/main.cpp index d78b931e2..77a2d985d 100644 --- a/gantry/simulator/main.cpp +++ b/gantry/simulator/main.cpp @@ -12,6 +12,8 @@ void signal_handler(int signum) { exit(signum); } +static interfaces::diag0_handler call_diag0_handler = NULL; + int main(int argc, char** argv) { signal(SIGINT, signal_handler); @@ -21,10 +23,10 @@ int main(int argc, char** argv) { return pcTaskGetName(xTaskGetCurrentTaskHandle()); }); - interfaces::initialize(); + interfaces::initialize(&call_diag0_handler); interfaces::initialize_sim(argc, argv); - gantry::tasks::start_tasks( + call_diag0_handler = gantry::tasks::start_tasks( interfaces::get_can_bus(), interfaces::get_motor().motion_controller, interfaces::get_spi(), interfaces::get_driver_config(), interfaces::get_motor_hardware_task(), *interfaces::get_sim_i2c2(), diff --git a/gripper/core/tasks.cpp b/gripper/core/tasks.cpp index 9c9ea66ed..ae3dd7d2b 100644 --- a/gripper/core/tasks.cpp +++ b/gripper/core/tasks.cpp @@ -62,7 +62,7 @@ static auto eeprom_data_rev_update_builder = /** * Start gripper tasks. */ -void gripper_tasks::start_tasks( +auto gripper_tasks::start_tasks( can::bus::CanBus& can_bus, motor_class::Motor<lms::LeadScrewConfig>& z_motor, brushed_motor::BrushedMotor<lms::GearBoxConfig>& grip_motor, @@ -72,7 +72,8 @@ void gripper_tasks::start_tasks( sensors::hardware::SensorHardwareBase& sensor_hardware, eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface, motor_hardware_task::MotorHardwareTask& zmh_tsk, - motor_hardware_task::MotorHardwareTask& gmh_tsk) { + motor_hardware_task::MotorHardwareTask& gmh_tsk) + -> z_motor_iface::diag0_handler { auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); tasks.can_writer = &can_writer; @@ -131,8 +132,8 @@ void gripper_tasks::start_tasks( queues.capacitive_sensor_queue_rear = &capacitive_sensor_task_rear.get_queue(); - z_tasks::start_task(z_motor, spi_device, driver_configs, tasks, queues, - tail_accessor); + auto diag0_handler = z_tasks::start_task( + z_motor, spi_device, driver_configs, tasks, queues, tail_accessor); g_tasks::start_task(grip_motor, tasks, queues, tail_accessor); @@ -141,6 +142,8 @@ void gripper_tasks::start_tasks( zmh_tsk.start_task(); gmh_tsk.start_task(); + + return diag0_handler; } gripper_tasks::QueueClient::QueueClient(can::ids::NodeId this_fw) diff --git a/gripper/core/tasks_z.cpp b/gripper/core/tasks_z.cpp index 43432ea46..ce5f9b306 100644 --- a/gripper/core/tasks_z.cpp +++ b/gripper/core/tasks_z.cpp @@ -34,15 +34,16 @@ static auto z_usage_storage_task_builder = freertos_task::TaskStarter<512, usage_storage_task::UsageStorageTask>{}; #endif -void z_tasks::start_task( +auto z_tasks::start_task( motor_class::Motor<lms::LeadScrewConfig>& z_motor, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, AllTask& gripper_tasks, gripper_tasks::QueueClient& main_queues, eeprom::dev_data::DevDataTailAccessor<gripper_tasks::QueueClient>& - tail_accessor) { - auto& motion = mc_task_builder.start(5, "z mc", z_motor.motion_controller, - z_queues, z_queues); + tail_accessor) -> z_motor_iface::diag0_handler { + auto& motion = + mc_task_builder.start(5, "z mc", z_motor.motion_controller, z_queues, + z_queues, z_queues, z_queues); auto& move_group = move_group_task_builder.start(5, "move group", z_queues, z_queues); auto& tmc2130_driver = motor_driver_task_builder.start( @@ -77,6 +78,14 @@ void z_tasks::start_task( z_queues.z_usage_storage_queue = &z_usage_storage_task.get_queue(); #endif spi_task_client.set_queue(&spi_task.get_queue()); + + return z_tasks::call_run_diag0_interrupt; +} + +void z_tasks::call_run_diag0_interrupt() { + if (get_all_tasks().motion_controller) { + return get_all_tasks().motion_controller->run_diag0_interrupt(); + } } z_tasks::QueueClient::QueueClient() @@ -92,6 +101,11 @@ void z_tasks::QueueClient::send_motor_driver_queue( tmc2130_driver_queue->try_write(m); } +void z_tasks::QueueClient::send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) { + static_cast<void>(tmc2130_driver_queue->try_write_isr(m)); +} + void z_tasks::QueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/gripper/firmware/interfaces_z_motor.cpp b/gripper/firmware/interfaces_z_motor.cpp index 69405dfde..a979ffca0 100644 --- a/gripper/firmware/interfaces_z_motor.cpp +++ b/gripper/firmware/interfaces_z_motor.cpp @@ -103,7 +103,13 @@ struct motion_controller::HardwareConfig motor_pins { .port = ESTOP_IN_PORT, .pin = ESTOP_IN_PIN, .active_setting = GPIO_PIN_RESET}, - .ebrake = ebrake, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_2, + .active_setting = GPIO_PIN_RESET}, + .ebrake = ebrake }; /** @@ -119,6 +125,7 @@ static motor_hardware::MotorHardware motor_hardware_iface(motor_pins, &htim7, */ static tmc2130::configs::TMC2130DriverConfig MotorDriverConfigurations{ .registers = {.gconfig = {.en_pwm_mode = 0x0, + .diag0_error = 1, .stop_enable = use_stop_enable}, .ihold_irun = {.hold_current = 0x2, // 0.177A .run_current = 0xA, // 0.648A @@ -196,8 +203,9 @@ static motor_class::Motor z_motor{ * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gripper_tasks::z_tasks::get_queues(), motor_hardware_iface, - stallcheck, update_position_queue); + motor_queue, gripper_tasks::z_tasks::get_queues(), + gripper_tasks::z_tasks::get_queues(), motor_hardware_iface, stallcheck, + update_position_queue); static auto encoder_background_timer = motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface); @@ -210,12 +218,13 @@ extern "C" void call_enc_handler(int32_t direction) { motor_hardware_iface.encoder_overflow(direction); } -void z_motor_iface::initialize() { +void z_motor_iface::initialize(diag0_handler* call_diag0_handler) { if (initialize_spi() != HAL_OK) { Error_Handler(); } initialize_hardware_z(); - set_z_motor_timer_callback(call_motor_handler, call_enc_handler); + set_z_motor_timer_callback(call_motor_handler, call_diag0_handler, + call_enc_handler); encoder_background_timer.start(); } diff --git a/gripper/firmware/main_proto.cpp b/gripper/firmware/main_proto.cpp index 96328dca9..c4a249b0c 100644 --- a/gripper/firmware/main_proto.cpp +++ b/gripper/firmware/main_proto.cpp @@ -25,6 +25,8 @@ #include "i2c/firmware/i2c_comms.hpp" #include "sensors/firmware/sensor_hardware.hpp" +static z_motor_iface::diag0_handler call_diag0_handler = nullptr; + static auto iWatchdog = iwdg::IndependentWatchDog{}; /** @@ -100,7 +102,7 @@ auto main() -> int { app_update_clear_flags(); - z_motor_iface::initialize(); + z_motor_iface::initialize(&call_diag0_handler); grip_motor_iface::initialize(); i2c_setup(&i2c_handles); @@ -108,7 +110,7 @@ auto main() -> int { i2c_comms3.set_handle(i2c_handles.i2c3); canbus.start(can_bit_timings); - gripper_tasks::start_tasks( + call_diag0_handler = gripper_tasks::start_tasks( canbus, z_motor_iface::get_z_motor(), grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(), z_motor_iface::get_tmc2130_driver_configs(), i2c_comms2, i2c_comms3, diff --git a/gripper/firmware/main_rev1.cpp b/gripper/firmware/main_rev1.cpp index b15eafec9..5e351ee53 100644 --- a/gripper/firmware/main_rev1.cpp +++ b/gripper/firmware/main_rev1.cpp @@ -26,6 +26,8 @@ #include "i2c/firmware/i2c_comms.hpp" #include "sensors/firmware/sensor_hardware.hpp" +static z_motor_iface::diag0_handler call_diag0_handler = nullptr; + static auto iWatchdog = iwdg::IndependentWatchDog{}; /** @@ -111,7 +113,7 @@ auto main() -> int { app_update_clear_flags(); - z_motor_iface::initialize(); + z_motor_iface::initialize(&call_diag0_handler); grip_motor_iface::initialize(); i2c_setup(&i2c_handles); @@ -119,7 +121,7 @@ auto main() -> int { i2c_comms3.set_handle(i2c_handles.i2c3); canbus.start(can_bit_timings); - gripper_tasks::start_tasks( + call_diag0_handler = gripper_tasks::start_tasks( canbus, z_motor_iface::get_z_motor(), grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(), z_motor_iface::get_tmc2130_driver_configs(), i2c_comms2, i2c_comms3, diff --git a/gripper/firmware/motor_hardware.h b/gripper/firmware/motor_hardware.h index 37bce25ca..4df6dbd0a 100644 --- a/gripper/firmware/motor_hardware.h +++ b/gripper/firmware/motor_hardware.h @@ -32,6 +32,7 @@ extern TIM_HandleTypeDef htim8; typedef void (*motor_interrupt_callback)(); typedef void (*z_encoder_overflow_callback)(int32_t); +typedef void (*diag0_interrupt_callback)(); HAL_StatusTypeDef initialize_spi(); @@ -39,6 +40,7 @@ void initialize_hardware_z(); void set_z_motor_timer_callback( motor_interrupt_callback callback, + diag0_interrupt_callback* diag0_int_callback, z_encoder_overflow_callback enc_callback); diff --git a/gripper/firmware/motor_hardware_shared.c b/gripper/firmware/motor_hardware_shared.c index 997322797..f6118af8b 100644 --- a/gripper/firmware/motor_hardware_shared.c +++ b/gripper/firmware/motor_hardware_shared.c @@ -3,6 +3,7 @@ static motor_interrupt_callback timer_callback = NULL; static z_encoder_overflow_callback z_enc_overflow_callback = NULL; +static diag0_interrupt_callback* diag0_callback = NULL; static brushed_motor_interrupt_callback brushed_timer_callback = NULL; static encoder_overflow_callback gripper_enc_overflow_callback = NULL; static encoder_idle_state_callback gripper_enc_idle_state_overflow_callback = @@ -10,11 +11,19 @@ static encoder_idle_state_callback gripper_enc_idle_state_overflow_callback = static stopwatch_overflow_callback gripper_force_stopwatch_overflow_callback = NULL; +void MX_GPIO_Init(void) { + HAL_NVIC_SetPriority(EXTI2_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(EXTI2_IRQn); +} + void set_z_motor_timer_callback( motor_interrupt_callback callback, + diag0_interrupt_callback* diag0_int_callback, z_encoder_overflow_callback enc_callback) { timer_callback = callback; z_enc_overflow_callback = enc_callback; + diag0_callback = diag0_int_callback; + MX_GPIO_Init(); } void set_brushed_motor_timer_callback( @@ -212,9 +221,14 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef* htim) { } } - void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { - if (GPIO_Pin == ESTOP_IN_PIN) { + if (GPIO_Pin == GPIO_PIN_2) { + if (diag0_callback != NULL) { + if (*diag0_callback != NULL) { + (*diag0_callback)(); + } + } + } else if (GPIO_Pin == ESTOP_IN_PIN) { #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a' HAL_GPIO_WritePin(EBRAKE_PORT, EBRAKE_PIN, GPIO_PIN_RESET); #endif diff --git a/gripper/firmware/stm32g4xx_it.c b/gripper/firmware/stm32g4xx_it.c index 2051ebc6b..6a90023e6 100644 --- a/gripper/firmware/stm32g4xx_it.c +++ b/gripper/firmware/stm32g4xx_it.c @@ -198,6 +198,11 @@ void TIM7_IRQHandler(void) { call_motor_handler(); } +__attribute__((section(".ccmram"))) +void EXTI2_IRQHandler(void) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_2); +} + /** * @brief This function handles EXTI line[15:10] interrupts. */ diff --git a/gripper/firmware/utility_gpio.c b/gripper/firmware/utility_gpio.c index 551f5cc12..9c25e0b46 100644 --- a/gripper/firmware/utility_gpio.c +++ b/gripper/firmware/utility_gpio.c @@ -108,6 +108,8 @@ static void z_motor_gpio_init(void) { PB1 ------> Motor Step Pin Enable PA9 ------> Motor Enable Pin + Diag0 + PB2 ------> Motor Diag0 Pin */ GPIO_InitStruct.Pin = Z_MOT_DIR_PIN | Z_MOT_STEP_PIN; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; @@ -119,6 +121,11 @@ static void z_motor_gpio_init(void) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(Z_MOT_ENABLE_PORT, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + GPIO_InitStruct.Pin = Z_MOT_DIAG0_PIN; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(Z_MOT_STEPDIR_PORT, &GPIO_InitStruct); } #if PCBA_PRIMARY_REVISION != 'b' && PCBA_PRIMARY_REVISION != 'a' diff --git a/gripper/simulator/interfaces.cpp b/gripper/simulator/interfaces.cpp index 085c4ad77..104d0575a 100644 --- a/gripper/simulator/interfaces.cpp +++ b/gripper/simulator/interfaces.cpp @@ -101,8 +101,9 @@ static stall_check::StallCheck stallcheck( * Handler of motor interrupts. */ static motor_handler::MotorInterruptHandler motor_interrupt( - motor_queue, gripper_tasks::z_tasks::get_queues(), motor_interface, - stallcheck, update_position_queue); + motor_queue, gripper_tasks::z_tasks::get_queues(), + gripper_tasks::z_tasks::get_queues(), motor_interface, stallcheck, + update_position_queue); static motor_interrupt_driver::MotorInterruptDriver A(motor_queue, motor_interrupt, @@ -142,7 +143,8 @@ static brushed_motor_handler::BrushedMotorInterruptHandler static brushed_motor_interrupt_driver::BrushedMotorInterruptDriver G( brushed_motor_queue, brushed_motor_interrupt, brushed_motor_hardware_iface); -void z_motor_iface::initialize() { +void z_motor_iface::initialize(diag0_handler* call_diag0_handler) { + static_cast<void>(call_diag0_handler); motor_interface.provide_mech_config(z_motor_sys_config); }; diff --git a/gripper/simulator/main.cpp b/gripper/simulator/main.cpp index 23ff9edff..6b2a93db5 100644 --- a/gripper/simulator/main.cpp +++ b/gripper/simulator/main.cpp @@ -21,6 +21,8 @@ namespace po = boost::program_options; +static z_motor_iface::diag0_handler call_diag0_handler = nullptr; + void signal_handler(int signum) { LOG("Interrupt signal (%d) received.", signum); exit(signum); @@ -114,9 +116,9 @@ int main(int argc, char** argv) { auto i2c3 = std::make_shared<i2c::hardware::SimI2C>(i2c_device_map); static auto canbus = can::sim::bus::SimCANBus(can::sim::transport::create(options)); - z_motor_iface::initialize(); + z_motor_iface::initialize(&call_diag0_handler); grip_motor_iface::initialize(); - gripper_tasks::start_tasks( + call_diag0_handler = gripper_tasks::start_tasks( canbus, z_motor_iface::get_z_motor(), grip_motor_iface::get_grip_motor(), z_motor_iface::get_spi(), z_motor_iface::get_tmc2130_driver_configs(), i2c2, *i2c3, diff --git a/head/core/can_task.cpp b/head/core/can_task.cpp index 6cc432b07..dcb1f3359 100644 --- a/head/core/can_task.cpp +++ b/head/core/can_task.cpp @@ -28,7 +28,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler<head_tasks::MotorQueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler< head_tasks::MotorQueueClient>, diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp index 2690550ca..c3c09d968 100644 --- a/head/core/tasks_proto.cpp +++ b/head/core/tasks_proto.cpp @@ -78,10 +78,25 @@ static auto eeprom_data_rev_update_builder = freertos_task::TaskStarter<512, eeprom::data_rev_task::UpdateDataRevTask>{}; static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{head_queues}; + +void call_run_diag0_z_interrupt() { + if (head_tasks::get_left_tasks().motion_controller) { + return head_tasks::get_left_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + +void call_run_diag0_a_interrupt() { + if (head_tasks::get_right_tasks().motion_controller) { + return head_tasks::get_right_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + /** * Start head tasks. */ -void head_tasks::start_tasks( +auto head_tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::LeadScrewConfig>& left_motion_controller, @@ -95,7 +110,8 @@ void head_tasks::start_tasks( motor_hardware_task::MotorHardwareTask& rmh_tsk, motor_hardware_task::MotorHardwareTask& lmh_tsk, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple<diag0_handler, diag0_handler> { // Start the head tasks auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); @@ -141,7 +157,8 @@ void head_tasks::start_tasks( // Start the left motor tasks auto& left_motion = left_mc_task_builder.start( - 5, "left mc", left_motion_controller, left_queues, left_queues); + 5, "left mc", left_motion_controller, left_queues, left_queues, + left_queues, left_queues); auto& left_tmc2130_driver = left_motor_driver_task_builder.start( 5, "left motor driver", left_driver_configs, left_queues, spi3_task_client); @@ -170,7 +187,8 @@ void head_tasks::start_tasks( // Start the right motor tasks auto& right_motion = right_mc_task_builder.start( - 5, "right mc", right_motion_controller, right_queues, right_queues); + 5, "right mc", right_motion_controller, right_queues, right_queues, + right_queues, right_queues); auto& right_tmc2130_driver = right_motor_driver_task_builder.start( 5, "right motor driver", right_driver_configs, right_queues, spi2_task_client); @@ -199,6 +217,9 @@ void head_tasks::start_tasks( right_queues.move_status_report_queue = &right_move_status_reporter.get_queue(); right_queues.usage_storage_queue = &right_usage_storage_task.get_queue(); + + return std::make_tuple(call_run_diag0_z_interrupt, + call_run_diag0_a_interrupt); } // Implementation of HeadQueueClient @@ -231,6 +252,11 @@ void head_tasks::MotorQueueClient::send_motor_driver_queue( motor_queue->try_write(m); } +void head_tasks::MotorQueueClient::send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) { + static_cast<void>(motor_queue->try_write_isr(m)); +} + void head_tasks::MotorQueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp index 2cf2d803d..3e417f116 100644 --- a/head/core/tasks_rev1.cpp +++ b/head/core/tasks_rev1.cpp @@ -83,10 +83,25 @@ static auto right_usage_storage_task_builder = static auto eeprom_data_rev_update_builder = freertos_task::TaskStarter<512, eeprom::data_rev_task::UpdateDataRevTask>{}; #endif + +void call_run_diag0_z_interrupt() { + if (head_tasks::get_left_tasks().motion_controller) { + return head_tasks::get_left_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + +void call_run_diag0_a_interrupt() { + if (head_tasks::get_right_tasks().motion_controller) { + return head_tasks::get_right_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + /** * Start head tasks. */ -void head_tasks::start_tasks( +auto head_tasks::start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::LeadScrewConfig>& left_motion_controller, @@ -100,7 +115,8 @@ void head_tasks::start_tasks( motor_hardware_task::MotorHardwareTask& rmh_tsk, motor_hardware_task::MotorHardwareTask& lmh_tsk, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) { + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple<head_tasks::diag0_handler, head_tasks::diag0_handler> { // Start the head tasks auto& can_writer = can_task::start_writer(can_bus); can_task::start_reader(can_bus); @@ -146,7 +162,8 @@ void head_tasks::start_tasks( #endif // Start the left motor tasks auto& left_motion = left_mc_task_builder.start( - 5, "left mc", left_motion_controller, left_queues, left_queues); + 5, "left mc", left_motion_controller, left_queues, left_queues, + left_queues, left_queues); auto& left_tmc2160_driver = left_motor_driver_task_builder.start( 5, "left motor driver", left_driver_configs, left_queues, spi3_task_client); @@ -183,7 +200,8 @@ void head_tasks::start_tasks( // Start the right motor tasks auto& right_motion = right_mc_task_builder.start( - 5, "right mc", right_motion_controller, right_queues, right_queues); + 5, "right mc", right_motion_controller, right_queues, right_queues, + right_queues, right_queues); auto& right_tmc2160_driver = right_motor_driver_task_builder.start( 5, "right motor driver", right_driver_configs, right_queues, spi2_task_client); @@ -220,6 +238,9 @@ void head_tasks::start_tasks( #if PCBA_PRIMARY_REVISION != 'b' right_queues.usage_storage_queue = &right_usage_storage_task.get_queue(); #endif + + return std::make_tuple(call_run_diag0_z_interrupt, + call_run_diag0_a_interrupt); } // Implementation of HeadQueueClient @@ -256,6 +277,11 @@ void head_tasks::MotorQueueClient::send_motor_driver_queue( motor_queue->try_write(m); } +void head_tasks::MotorQueueClient::send_motor_driver_queue_isr( + const tmc::tasks::TaskMessage& m) { + static_cast<void>(motor_queue->try_write_isr(m)); +} + void head_tasks::MotorQueueClient::send_move_group_queue( const move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp index d3ba07641..614af1142 100644 --- a/head/firmware/main_proto.cpp +++ b/head/firmware/main_proto.cpp @@ -1,6 +1,7 @@ #include <array> #include <cstdio> #include <cstring> +#include <tuple> // clang-format off #include "FreeRTOS.h" @@ -40,6 +41,9 @@ static auto iWatchdog = iwdg::IndependentWatchDog{}; +static head_tasks::diag0_handler call_diag0_z_handler = nullptr; +static head_tasks::diag0_handler call_diag0_a_handler = nullptr; + static auto can_bus_1 = can::hal::bus::HalCanBus( can_get_device_handle(), gpio::PinConfig{// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -142,10 +146,16 @@ struct motor_hardware::HardwareConfig pin_configurations_left { .port = GPIOA, .pin = GPIO_PIN_8, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_4, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOB, - .pin = GPIO_PIN_4, + .port = GPIOC, + .pin = GPIO_PIN_13, .active_setting = GPIO_PIN_RESET} }; @@ -186,10 +196,16 @@ struct motor_hardware::HardwareConfig pin_configurations_right { .port = GPIOA, .pin = GPIO_PIN_8, .active_setting = GPIO_PIN_RESET}, - .estop_in = { + .estop_in = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_4, + .active_setting = GPIO_PIN_RESET}, + .diag0 = { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) - .port = GPIOB, - .pin = GPIO_PIN_4, + .port = GPIOC, + .pin = GPIO_PIN_15, .active_setting = GPIO_PIN_RESET} }; @@ -197,7 +213,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right { static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{ .registers = { - .gconfig = {.en_pwm_mode = 1}, + .gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0xB, .run_current = 0x19, .hold_current_delay = 0x7}, @@ -224,7 +240,7 @@ static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{ static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_left{ .registers = { - .gconfig = {.en_pwm_mode = 1}, + .gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0xB, .run_current = 0x19, .hold_current_delay = 0x7}, @@ -268,8 +284,9 @@ static stall_check::StallCheck stallcheck_right( static motor_hardware::MotorHardware motor_hardware_right( pin_configurations_right, &htim7, &htim2, right_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_right( - motor_queue_right, head_tasks::get_right_queues(), motor_hardware_right, - stallcheck_right, update_position_queue_right); + motor_queue_right, head_tasks::get_right_queues(), + head_tasks::get_right_queues(), motor_hardware_right, stallcheck_right, + update_position_queue_right); static auto encoder_background_timer_right = motor_encoder::BackgroundTimer(motor_interrupt_right, motor_hardware_right); @@ -289,8 +306,9 @@ static stall_check::StallCheck stallcheck_left( static motor_hardware::MotorHardware motor_hardware_left( pin_configurations_left, &htim7, &htim3, left_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_left( - motor_queue_left, head_tasks::get_left_queues(), motor_hardware_left, - stallcheck_left, update_position_queue_left); + motor_queue_left, head_tasks::get_left_queues(), + head_tasks::get_left_queues(), motor_hardware_left, stallcheck_left, + update_position_queue_left); static auto encoder_background_timer_left = motor_encoder::BackgroundTimer(motor_interrupt_left, motor_hardware_left); @@ -393,7 +411,8 @@ auto main() -> int { app_update_clear_flags(); initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue, - right_enc_overflow_callback_glue); + right_enc_overflow_callback_glue, &call_diag0_z_handler, + &call_diag0_a_handler); if (initialize_spi(&hspi2) != HAL_OK) { Error_Handler(); @@ -407,12 +426,12 @@ auto main() -> int { i2c_setup(&i2c_handles); i2c_comms3.set_handle(i2c_handles.i2c3); - - head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, - motor_right.motion_controller, psd, spi_comms2, - spi_comms3, motor_driver_configs_left, - motor_driver_configs_right, rmh_tsk, lmh_tsk, - i2c_comms3, eeprom_hw_iface); + std::tie(call_diag0_z_handler, call_diag0_a_handler) = + head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, + motor_right.motion_controller, psd, spi_comms2, + spi_comms3, motor_driver_configs_left, + motor_driver_configs_right, rmh_tsk, lmh_tsk, + i2c_comms3, eeprom_hw_iface); timer_for_notifier.start(); diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp index ce5849129..e6d17776f 100644 --- a/head/firmware/main_rev1.cpp +++ b/head/firmware/main_rev1.cpp @@ -1,6 +1,7 @@ #include <array> #include <cstdio> #include <cstring> +#include <tuple> // clang-format off #include "FreeRTOS.h" @@ -40,6 +41,9 @@ static auto iWatchdog = iwdg::IndependentWatchDog{}; +static head_tasks::diag0_handler call_diag0_z_handler = nullptr; +static head_tasks::diag0_handler call_diag0_a_handler = nullptr; + static auto can_bus_1 = can::hal::bus::HalCanBus( can_get_device_handle(), gpio::PinConfig{// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) @@ -154,6 +158,12 @@ struct motor_hardware::HardwareConfig pin_configurations_left { .port = GPIOB, .pin = GPIO_PIN_4, .active_setting = GPIO_PIN_RESET}, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_13, + .active_setting = GPIO_PIN_RESET}, .ebrake = gpio::PinConfig { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOB, .pin = GPIO_PIN_5, .active_setting = GPIO_PIN_RESET @@ -198,6 +208,12 @@ struct motor_hardware::HardwareConfig pin_configurations_right { .port = GPIOB, .pin = GPIO_PIN_4, .active_setting = GPIO_PIN_RESET}, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_15, + .active_setting = GPIO_PIN_RESET}, .ebrake = gpio::PinConfig { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOB, .pin = GPIO_PIN_0, .active_setting = GPIO_PIN_RESET @@ -206,7 +222,7 @@ struct motor_hardware::HardwareConfig pin_configurations_right { // TODO clean up the head main file by using interfaces. static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -232,7 +248,7 @@ static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_right{ }}; static tmc2160::configs::TMC2160DriverConfig motor_driver_configs_left{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -286,8 +302,9 @@ static stall_check::StallCheck stallcheck_right( static motor_hardware::MotorHardware motor_hardware_right( pin_configurations_right, &htim7, &htim2, right_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_right( - motor_queue_right, head_tasks::get_right_queues(), motor_hardware_right, - stallcheck_right, update_position_queue_right); + motor_queue_right, head_tasks::get_right_queues(), + head_tasks::get_right_queues(), motor_hardware_right, stallcheck_right, + update_position_queue_right); static auto encoder_background_timer_right = motor_encoder::BackgroundTimer(motor_interrupt_right, motor_hardware_right); @@ -311,8 +328,9 @@ static stall_check::StallCheck stallcheck_left( static motor_hardware::MotorHardware motor_hardware_left( pin_configurations_left, &htim7, &htim3, left_usage_config); static motor_handler::MotorInterruptHandler motor_interrupt_left( - motor_queue_left, head_tasks::get_left_queues(), motor_hardware_left, - stallcheck_left, update_position_queue_left); + motor_queue_left, head_tasks::get_left_queues(), + head_tasks::get_left_queues(), motor_hardware_left, stallcheck_left, + update_position_queue_left); static auto encoder_background_timer_left = motor_encoder::BackgroundTimer(motor_interrupt_left, motor_hardware_left); @@ -418,7 +436,8 @@ auto main() -> int { app_update_clear_flags(); initialize_timer(motor_callback_glue, left_enc_overflow_callback_glue, - right_enc_overflow_callback_glue); + right_enc_overflow_callback_glue, &call_diag0_z_handler, + &call_diag0_a_handler); i2c_setup(&i2c_handles); i2c_comms3.set_handle(i2c_handles.i2c3); @@ -432,11 +451,12 @@ auto main() -> int { utility_gpio_init(); can_bus_1.start(can_bit_timings); - head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, - motor_right.motion_controller, psd, spi_comms2, - spi_comms3, motor_driver_configs_left, - motor_driver_configs_right, rmh_tsk, lmh_tsk, - i2c_comms3, eeprom_hw_iface); + std::tie(call_diag0_z_handler, call_diag0_a_handler) = + head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, + motor_right.motion_controller, psd, spi_comms2, + spi_comms3, motor_driver_configs_left, + motor_driver_configs_right, rmh_tsk, lmh_tsk, + i2c_comms3, eeprom_hw_iface); timer_for_notifier.start(); diff --git a/head/firmware/motor_hardware.h b/head/firmware/motor_hardware.h index e01e68d85..2cce7382f 100644 --- a/head/firmware/motor_hardware.h +++ b/head/firmware/motor_hardware.h @@ -14,11 +14,14 @@ extern TIM_HandleTypeDef htim3; typedef void (*motor_interrupt_callback)(); typedef void (*encoder_overflow_callback)(int32_t); +typedef void (*diag0_interrupt_callback)(); HAL_StatusTypeDef initialize_spi(SPI_HandleTypeDef* hspi); void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback left_enc_overflow_callback, - encoder_overflow_callback right_enc_overflow_callback); + encoder_overflow_callback right_enc_overflow_callback, + diag0_interrupt_callback* diag0_z_int_callback, + diag0_interrupt_callback* diag0_a_int_callback); void initialize_rev_specific_pins(); #ifdef __cplusplus diff --git a/head/firmware/motor_hardware_common.c b/head/firmware/motor_hardware_common.c index d1313df4d..e895252fe 100644 --- a/head/firmware/motor_hardware_common.c +++ b/head/firmware/motor_hardware_common.c @@ -21,6 +21,8 @@ TIM_HandleTypeDef htim3 = { motor_interrupt_callback motor_callback = NULL; encoder_overflow_callback left_enc_overflow_callback = NULL; encoder_overflow_callback right_enc_overflow_callback = NULL; +static diag0_interrupt_callback* diag0_z_callback = NULL; +static diag0_interrupt_callback* diag0_a_callback = NULL; void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) { GPIO_InitTypeDef GPIO_InitStruct = {0}; @@ -55,6 +57,14 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) { GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + // A motor diag0 + GPIO_InitStruct.Pin = GPIO_PIN_15; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) + &GPIO_InitStruct); } else if (hspi->Instance == SPI3) { @@ -91,6 +101,14 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) { GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) &GPIO_InitStruct); + + // Z motor diag0 + GPIO_InitStruct.Pin = GPIO_PIN_13; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, // NOLINT(cppcoreguidelines-pro-type-cstyle-cast) + &GPIO_InitStruct); } } @@ -229,6 +247,9 @@ void MX_GPIO_Init(void) { __HAL_RCC_GPIOA_CLK_ENABLE(); __HAL_RCC_GPIOB_CLK_ENABLE(); initialize_rev_specific_pins(); + + HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); } #if PCBA_PRIMARY_REVISION == 'b' || PCBA_PRIMARY_REVISION == 'a' @@ -345,15 +366,31 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { // disable both left and right enable pins HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4, GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOB, GPIO_PIN_11, GPIO_PIN_RESET); + } else if (GPIO_Pin == GPIO_PIN_13) { + if (diag0_z_callback != NULL) { + if (*diag0_z_callback != NULL) { + (*diag0_z_callback)(); + } + } + } else if (GPIO_Pin == GPIO_PIN_15) { + if (diag0_a_callback != NULL) { + if (*diag0_a_callback != NULL) { + (*diag0_a_callback)(); + } + } } } void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback l_f_callback, - encoder_overflow_callback r_f_callback) { + encoder_overflow_callback r_f_callback, + diag0_interrupt_callback* diag0_z_int_callback, + diag0_interrupt_callback* diag0_a_int_callback) { motor_callback = callback; left_enc_overflow_callback = l_f_callback; right_enc_overflow_callback = r_f_callback; + diag0_z_callback = diag0_z_int_callback; + diag0_a_callback = diag0_a_int_callback; MX_GPIO_Init(); Encoder_GPIO_Init(); encoder_init(&htim2); diff --git a/head/firmware/stm32g4xx_it.c b/head/firmware/stm32g4xx_it.c index 6e031a2bf..929586b89 100644 --- a/head/firmware/stm32g4xx_it.c +++ b/head/firmware/stm32g4xx_it.c @@ -177,6 +177,14 @@ void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); } */ void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); } +void EXTI15_10_IRQHandler(void) { + if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_13)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13); + } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_15)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); + } +} + extern void xPortSysTickHandler(void); void SysTick_Handler(void) { HAL_IncTick(); diff --git a/head/simulator/main.cpp b/head/simulator/main.cpp index 79db8f736..907f60bdb 100644 --- a/head/simulator/main.cpp +++ b/head/simulator/main.cpp @@ -94,8 +94,9 @@ static stall_check::StallCheck stallcheck_right( linear_config.get_usteps_per_mm() / 1000.0F, utils::STALL_THRESHOLD_UM); static motor_handler::MotorInterruptHandler motor_interrupt_right( - motor_queue_right, head_tasks::get_right_queues(), motor_interface_right, - stallcheck_right, update_position_queue_right); + motor_queue_right, head_tasks::get_right_queues(), + head_tasks::get_right_queues(), motor_interface_right, stallcheck_right, + update_position_queue_right); static stall_check::StallCheck stallcheck_left( linear_config.get_encoder_pulses_per_mm() / 1000.0F, @@ -117,8 +118,9 @@ static motor_class::Motor motor_right{ motor_queue_right, update_position_queue_right}; static motor_handler::MotorInterruptHandler motor_interrupt_left( - motor_queue_left, head_tasks::get_left_queues(), motor_interface_left, - stallcheck_left, update_position_queue_left); + motor_queue_left, head_tasks::get_left_queues(), + head_tasks::get_left_queues(), motor_interface_left, stallcheck_left, + update_position_queue_left); static motor_class::Motor motor_left{ motor_sys_config, motor_interface_left, diff --git a/include/can/core/ids.hpp b/include/can/core/ids.hpp index 636a70cdd..d5b3a2d48 100644 --- a/include/can/core/ids.hpp +++ b/include/can/core/ids.hpp @@ -64,6 +64,8 @@ enum class MessageId { write_motor_current_request = 0x33, read_motor_current_request = 0x34, read_motor_current_response = 0x35, + read_motor_driver_error_status_request = 0x36, + read_motor_driver_error_status_response = 0x37, set_brushed_motor_vref_request = 0x40, set_brushed_motor_pwm_request = 0x41, gripper_grip_request = 0x42, @@ -169,11 +171,13 @@ enum class ErrorCode { over_pressure = 0xd, door_open = 0xe, reed_open = 0xf, + motor_driver_error_detected = 0x10, safety_relay_inactive = 0x11, }; /** Error Severity levels. */ enum class ErrorSeverity { + none = 0x0, warning = 0x1, recoverable = 0x2, unrecoverable = 0x3, diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 3ad1da7c5..71654f7de 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -646,6 +646,41 @@ struct ReadMotorDriverRegisterResponse -> bool = default; }; +struct ReadMotorDriverErrorStatusRequest + : BaseMessage<MessageId::read_motor_driver_error_status_request> { + uint32_t message_index; + + template <bit_utils::ByteIterator Input, typename Limit> + static auto parse(Input body, Limit limit) + -> ReadMotorDriverErrorStatusRequest { + uint32_t msg_ind = 0; + + body = bit_utils::bytes_to_int(body, limit, msg_ind); + return ReadMotorDriverErrorStatusRequest{.message_index = msg_ind}; + } + + auto operator==(const ReadMotorDriverErrorStatusRequest& other) const + -> bool = default; +}; + +struct ReadMotorDriverErrorStatusResponse + : BaseMessage<MessageId::read_motor_driver_error_status_response> { + uint32_t message_index; + uint8_t reg_address; + uint32_t data; + + template <bit_utils::ByteIterator Output, typename Limit> + auto serialize(Output body, Limit limit) const -> uint8_t { + auto iter = bit_utils::int_to_bytes(message_index, body, limit); + iter = bit_utils::int_to_bytes(reg_address, iter, limit); + iter = bit_utils::int_to_bytes(data, iter, limit); + return iter - body; + } + + auto operator==(const ReadMotorDriverErrorStatusResponse& other) const + -> bool = default; +}; + struct WriteMotorCurrentRequest : BaseMessage<MessageId::write_motor_current_request> { uint32_t message_index; @@ -1851,6 +1886,7 @@ using ResponseMessageType = std::variant< UpdateMotorPositionEstimationResponse, BaselineSensorResponse, PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse, GripperJawHoldoffResponse, HepaUVInfoResponse, GetHepaFanStateResponse, - GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse>; + GetHepaUVStateResponse, MotorStatusResponse, GearMotorStatusResponse, + ReadMotorDriverErrorStatusResponse>; } // namespace can::messages diff --git a/include/gantry/core/can_task.hpp b/include/gantry/core/can_task.hpp index 2c4918fff..53bf495f9 100644 --- a/include/gantry/core/can_task.hpp +++ b/include/gantry/core/can_task.hpp @@ -21,7 +21,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler<gantry::queues::QueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler< gantry::queues::QueueClient>, diff --git a/include/gantry/core/interfaces_proto.hpp b/include/gantry/core/interfaces_proto.hpp index d58ea63fd..ad72967b2 100644 --- a/include/gantry/core/interfaces_proto.hpp +++ b/include/gantry/core/interfaces_proto.hpp @@ -9,10 +9,12 @@ namespace interfaces { +extern "C" using diag0_handler = void(*)(); + /** * Initialize the hardware portability layer. */ -void initialize(); +void initialize(diag0_handler *call_diag0_handler); /** * Get the CAN bus interface. diff --git a/include/gantry/core/interfaces_rev1.hpp b/include/gantry/core/interfaces_rev1.hpp index 129d2ac68..fa276cbff 100644 --- a/include/gantry/core/interfaces_rev1.hpp +++ b/include/gantry/core/interfaces_rev1.hpp @@ -9,10 +9,12 @@ namespace interfaces { +extern "C" using diag0_handler = void(*)(); + /** * Initialize the hardware portability layer. */ -void initialize(); +void initialize(diag0_handler *call_diag0_handler); /** * Get the CAN bus interface. diff --git a/include/gantry/core/queues.hpp b/include/gantry/core/queues.hpp index 3007e49cb..42884aa01 100644 --- a/include/gantry/core/queues.hpp +++ b/include/gantry/core/queues.hpp @@ -32,6 +32,8 @@ struct QueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/gantry/core/tasks_proto.hpp b/include/gantry/core/tasks_proto.hpp index c9fe5be64..8fe6846d4 100644 --- a/include/gantry/core/tasks_proto.hpp +++ b/include/gantry/core/tasks_proto.hpp @@ -6,6 +6,7 @@ #include "eeprom/core/hardware_iface.hpp" #include "eeprom/core/task.hpp" #include "eeprom/core/update_data_rev_task.hpp" +#include "gantry/core/interfaces_proto.hpp" #include "i2c/core/hardware_iface.hpp" #include "i2c/core/tasks/i2c_poller_task.hpp" #include "i2c/core/tasks/i2c_task.hpp" @@ -28,14 +29,17 @@ namespace tasks { /** * Start gantry tasks. */ -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::BeltConfig>& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler; + +void call_run_diag0_interrupt(); /** * Access to all tasks in the system. diff --git a/include/gantry/core/tasks_rev1.hpp b/include/gantry/core/tasks_rev1.hpp index 0a90e559d..ea086c412 100644 --- a/include/gantry/core/tasks_rev1.hpp +++ b/include/gantry/core/tasks_rev1.hpp @@ -6,6 +6,7 @@ #include "eeprom/core/hardware_iface.hpp" #include "eeprom/core/task.hpp" #include "eeprom/core/update_data_rev_task.hpp" +#include "gantry/core/interfaces_rev1.hpp" #include "i2c/core/hardware_iface.hpp" #include "i2c/core/tasks/i2c_poller_task.hpp" #include "i2c/core/tasks/i2c_task.hpp" @@ -28,14 +29,17 @@ namespace tasks { /** * Start gantry tasks. */ -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::BeltConfig>& motion_controller, spi::hardware::SpiDeviceBase& spi_device, tmc2160::configs::TMC2160DriverConfig& driver_configs, motor_hardware_task::MotorHardwareTask& mh_tsk, i2c::hardware::I2CBase& i2c2, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> interfaces::diag0_handler; + +void call_run_diag0_interrupt(); /** * Access to all tasks in the system. diff --git a/include/gripper/core/can_task.hpp b/include/gripper/core/can_task.hpp index c31568ded..49a70ad44 100644 --- a/include/gripper/core/can_task.hpp +++ b/include/gripper/core/can_task.hpp @@ -18,7 +18,8 @@ using MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler<z_tasks::QueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; #ifdef USE_SENSOR_MOVE using MoveGroupDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::move_group::MoveGroupHandler<z_tasks::QueueClient>, diff --git a/include/gripper/core/interfaces.hpp b/include/gripper/core/interfaces.hpp index f11dac6ef..5430d5801 100644 --- a/include/gripper/core/interfaces.hpp +++ b/include/gripper/core/interfaces.hpp @@ -11,7 +11,9 @@ namespace z_motor_iface { -void initialize(); +extern "C" using diag0_handler = void(*)(); + +void initialize(diag0_handler *call_diag0_handler); /** * Access to the z motor. diff --git a/include/gripper/core/tasks.hpp b/include/gripper/core/tasks.hpp index c61ef164b..1de5f7c06 100644 --- a/include/gripper/core/tasks.hpp +++ b/include/gripper/core/tasks.hpp @@ -7,6 +7,7 @@ #include "eeprom/core/hardware_iface.hpp" #include "eeprom/core/task.hpp" #include "eeprom/core/update_data_rev_task.hpp" +#include "gripper/core/interfaces.hpp" #include "i2c/core/hardware_iface.hpp" #include "i2c/core/tasks/i2c_poller_task.hpp" #include "i2c/core/tasks/i2c_task.hpp" @@ -36,7 +37,7 @@ namespace gripper_tasks { /** * Start gripper tasks. */ -void start_tasks(can::bus::CanBus& can_bus, +auto start_tasks(can::bus::CanBus& can_bus, motor_class::Motor<lms::LeadScrewConfig>& z_motor, brushed_motor::BrushedMotor<lms::GearBoxConfig>& grip_motor, spi::hardware::SpiDeviceBase& spi_device, @@ -45,7 +46,8 @@ void start_tasks(can::bus::CanBus& can_bus, sensors::hardware::SensorHardwareBase& sensor_hardware, eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface, motor_hardware_task::MotorHardwareTask& zmh_tsk, - motor_hardware_task::MotorHardwareTask& gmh_tsk); + motor_hardware_task::MotorHardwareTask& gmh_tsk) + -> z_motor_iface::diag0_handler; /** * Access to all the message queues in the system. @@ -184,13 +186,15 @@ struct AllTask { namespace z_tasks { -void start_task( +auto start_task( motor_class::Motor<lms::LeadScrewConfig>& z_motor, spi::hardware::SpiDeviceBase& spi_device, tmc2130::configs::TMC2130DriverConfig& driver_configs, AllTask& tasks, gripper_tasks::QueueClient& main_queues, eeprom::dev_data::DevDataTailAccessor<gripper_tasks::QueueClient>& - tail_accessor); + tail_accessor) -> z_motor_iface::diag0_handler; + +void call_run_diag0_interrupt(); struct QueueClient : can::message_writer::MessageWriter { QueueClient(); @@ -200,6 +204,8 @@ struct QueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc2130::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc2130::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/gripper/firmware/utility_gpio.h b/include/gripper/firmware/utility_gpio.h index 4cb96d005..c0267033a 100644 --- a/include/gripper/firmware/utility_gpio.h +++ b/include/gripper/firmware/utility_gpio.h @@ -53,6 +53,7 @@ void utility_gpio_init(); // z motor pins #define Z_MOT_DIR_PIN GPIO_PIN_10 #define Z_MOT_STEP_PIN GPIO_PIN_1 +#define Z_MOT_DIAG0_PIN GPIO_PIN_2 #define Z_MOT_STEPDIR_PORT GPIOB #define Z_MOT_ENABLE_PIN GPIO_PIN_9 #define Z_MOT_ENABLE_PORT GPIOA diff --git a/include/gripper/simulation/sim_interfaces.hpp b/include/gripper/simulation/sim_interfaces.hpp index 4de28bad4..a3750ab63 100644 --- a/include/gripper/simulation/sim_interfaces.hpp +++ b/include/gripper/simulation/sim_interfaces.hpp @@ -4,6 +4,8 @@ namespace z_motor_iface { +extern "C" using diag0_handler = void(*)(); + auto get_z_motor_interface() -> sim_motor_hardware_iface::SimMotorHardwareIface&; diff --git a/include/head/core/queues.hpp b/include/head/core/queues.hpp index 2552275ff..6a07d9cd1 100644 --- a/include/head/core/queues.hpp +++ b/include/head/core/queues.hpp @@ -55,6 +55,8 @@ struct MotorQueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/head/core/tasks_proto.hpp b/include/head/core/tasks_proto.hpp index 6f87e5286..8be17643e 100644 --- a/include/head/core/tasks_proto.hpp +++ b/include/head/core/tasks_proto.hpp @@ -24,11 +24,12 @@ namespace head_tasks { +extern "C" using diag0_handler = void(*)(); + /** * Start head tasks. */ - -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::LeadScrewConfig>& left_motion_controller, @@ -42,7 +43,8 @@ void start_tasks( motor_hardware_task::MotorHardwareTask& right_motor_hardware, motor_hardware_task::MotorHardwareTask& left_motor_hardware, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple<diag0_handler, diag0_handler>; /** * Access to all tasks not associated with a motor. This will be a singleton. diff --git a/include/head/core/tasks_rev1.hpp b/include/head/core/tasks_rev1.hpp index bbb375586..430c89cf4 100644 --- a/include/head/core/tasks_rev1.hpp +++ b/include/head/core/tasks_rev1.hpp @@ -24,11 +24,12 @@ namespace head_tasks { +extern "C" using diag0_handler = void(*)(); + /** * Start head tasks. */ - -void start_tasks( +auto start_tasks( can::bus::CanBus& can_bus, motion_controller::MotionController<lms::LeadScrewConfig>& left_motion_controller, @@ -42,7 +43,8 @@ void start_tasks( motor_hardware_task::MotorHardwareTask& right_motor_hardware, motor_hardware_task::MotorHardwareTask& left_motor_hardware, i2c::hardware::I2CBase& i2c3, - eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface); + eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) + -> std::tuple<diag0_handler, diag0_handler>; /** * Access to all tasks not associated with a motor. This will be a singleton. 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 00ec0dddf..113f56214 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp @@ -101,12 +101,15 @@ class MotionController { enabled = false; } - void stop() { + 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(); + hardware.set_cancel_request(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 436311fad..634249d8c 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 @@ -180,7 +180,7 @@ class BrushedMotorInterruptHandler { } else if (in_estop) { // if we've received a stop request during this time we can clear // that flag since there isn't anything running - std::ignore = hardware.has_cancel_request(); + std::ignore = hardware.get_cancel_request(); // return out of error state once the estop is disabled if (!estop_triggered()) { in_estop = false; @@ -197,7 +197,9 @@ class BrushedMotorInterruptHandler { } else if (estop_triggered()) { in_estop = true; cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); - } else if (hardware.has_cancel_request()) { + } else if (static_cast<can::ids::ErrorCode>( + hardware.get_cancel_request().code) != + can::ids::ErrorCode::ok) { 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 9a979143d..647603250 100644 --- a/include/motor-control/core/motor_hardware_interface.hpp +++ b/include/motor-control/core/motor_hardware_interface.hpp @@ -72,6 +72,11 @@ class UsageEEpromConfig { size_t num_keys = 0; }; +struct __attribute__((packed)) CancelRequest { + uint8_t severity; + uint8_t code; +}; + class MotorHardwareIface { public: MotorHardwareIface() = default; @@ -90,6 +95,7 @@ class MotorHardwareIface { virtual void read_limit_switch() = 0; virtual void read_estop_in() = 0; virtual void read_sync_in() = 0; + virtual auto read_tmc_diag0() -> bool = 0; virtual auto get_encoder_pulses() -> int32_t = 0; virtual void reset_encoder_pulses() = 0; virtual void start_timer_interrupt() = 0; @@ -98,8 +104,10 @@ class MotorHardwareIface { virtual void enable_encoder() = 0; virtual void disable_encoder() = 0; - virtual auto has_cancel_request() -> bool = 0; - virtual void request_cancel() = 0; + virtual auto get_cancel_request() -> CancelRequest = 0; + virtual void set_cancel_request(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; // This variable can remain public because the only public methods diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index cc22efc48..6f3267fcc 100644 --- a/include/motor-control/core/stepper_motor/motion_controller.hpp +++ b/include/motor-control/core/stepper_motor/motion_controller.hpp @@ -12,6 +12,9 @@ #include "motor-control/core/types.hpp" #include "motor-control/core/utils.hpp" +constexpr uint32_t DIAG0_DEBOUNCE_REPS = 9; +constexpr uint32_t DIAG0_DEBOUNCE_DELAY = 100; + namespace motion_controller { using namespace motor_messages; @@ -184,13 +187,22 @@ class MotionController { return update_queue.try_write(can_msg); } - void stop() { + 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(); + hardware.set_cancel_request(error_severity, error_code); } } + void clear_cancel_request() { hardware.clear_cancel_request(); } + + auto is_timer_interrupt_running() -> bool { + return hardware.is_timer_interrupt_running(); + } + auto read_limit_switch() -> bool { return hardware.check_limit_switch(); } [[nodiscard]] auto read_motor_position() const { @@ -205,6 +217,8 @@ class MotionController { auto check_read_sync_line() -> bool { return hardware.check_sync_in(); } + auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); } + void enable_motor() { hardware.activate_motor(); hardware.start_timer_interrupt(); @@ -339,20 +353,31 @@ class PipetteMotionController { return false; } - void stop() { + 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(); + hardware.set_cancel_request(error_severity, error_code); } } + void clear_cancel_request() { hardware.clear_cancel_request(); } + + auto is_timer_interrupt_running() -> bool { + return hardware.is_timer_interrupt_running(); + } + auto read_limit_switch() -> bool { return hardware.check_limit_switch(); } auto check_read_sync_line() -> bool { return hardware.check_sync_in(); } + auto read_tmc_diag0() -> bool { return hardware.read_tmc_diag0(); } + void enable_motor() { hardware.start_timer_interrupt(); hardware.activate_motor(); 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 5196a7f2d..fab6d550c 100644 --- a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp +++ b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp @@ -49,7 +49,7 @@ using namespace motor_messages; */ template <template <class> class QueueImpl, class StatusClient, - typename MotorMoveMessage, typename MotorHardware> + class DriverClient, typename MotorMoveMessage, typename MotorHardware> requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> && std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware> class MotorInterruptHandler { @@ -60,11 +60,13 @@ class MotorInterruptHandler { MotorInterruptHandler() = delete; MotorInterruptHandler(MoveQueue& incoming_move_queue, StatusClient& outgoing_queue, + DriverClient& driver_queue, MotorHardware& hardware_iface, stall_check::StallCheck& stall, UpdatePositionQueue& incoming_update_position_queue) : move_queue(incoming_move_queue), status_queue_client(outgoing_queue), + driver_client(driver_queue), hardware(hardware_iface), stall_checker{stall}, update_position_queue(incoming_update_position_queue) { @@ -191,6 +193,8 @@ class MotorInterruptHandler { void run_interrupt() { // handle various error states + motor_hardware::CancelRequest cancel_request = + hardware.get_cancel_request(); std::ignore = hardware.get_encoder_pulses(); if (clear_queue_until_empty) { // If we were executing a move when estop asserted, and @@ -210,9 +214,11 @@ class MotorInterruptHandler { } else if (estop_triggered()) { cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); in_estop = true; - } else if (hardware.has_cancel_request()) { - cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, - can::ids::ErrorSeverity::warning); + } else if (static_cast<can::ids::ErrorCode>(cancel_request.code) != + can::ids::ErrorCode::ok) { + cancel_and_clear_moves( + static_cast<can::ids::ErrorCode>(cancel_request.code), + static_cast<can::ids::ErrorSeverity>(cancel_request.severity)); } else { // Normal Move logic run_normal_interrupt(); @@ -469,6 +475,11 @@ class MotorInterruptHandler { can::messages::ErrorMessage{.message_index = message_index, .severity = severity, .error_code = err_code}); + if (err_code == can::ids::ErrorCode::motor_driver_error_detected) { + driver_client.send_motor_driver_queue_isr( + can::messages::ReadMotorDriverErrorStatusRequest{ + .message_index = message_index}); + } if (err_code == can::ids::ErrorCode::collision_detected) { build_and_send_ack(AckMessageId::position_error); } @@ -675,6 +686,7 @@ class MotorInterruptHandler { q31_31 position_tracker{0}; MoveQueue& move_queue; StatusClient& status_queue_client; + DriverClient& driver_client; MotorHardware& hardware; stall_check::StallCheck& stall_checker; UpdatePositionQueue& update_position_queue; diff --git a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp index fefe1f049..7c3a74e7d 100644 --- a/include/motor-control/core/stepper_motor/tmc2130_driver.hpp +++ b/include/motor-control/core/stepper_motor/tmc2130_driver.hpp @@ -52,10 +52,11 @@ class TMC2130 { auto operator=(const TMC2130&& c) = delete; ~TMC2130() = default; - auto read(Registers addr, uint32_t command_data, uint32_t message_index) - -> void { + auto read(Registers addr, uint32_t command_data, uint32_t message_index, + uint8_t tags = 0) -> void { auto converted_addr = static_cast<uint8_t>(addr); - _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf, + uint32_t token = spi::utils::build_token(converted_addr, tags); + _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index); } diff --git a/include/motor-control/core/stepper_motor/tmc2160.hpp b/include/motor-control/core/stepper_motor/tmc2160.hpp index 5f656e3f5..1590535fb 100644 --- a/include/motor-control/core/stepper_motor/tmc2160.hpp +++ b/include/motor-control/core/stepper_motor/tmc2160.hpp @@ -505,6 +505,21 @@ struct __attribute__((packed, __may_alias__)) DriveStatus { uint32_t stst : 1 = 0; }; +struct __attribute__((packed, __may_alias__)) DriveConf { + static constexpr Registers address = Registers::DRV_CONF; + static constexpr bool writable = true; + static constexpr uint32_t value_mask = (1 << 22) - 1; + + uint32_t bbm_time : 5 = 0; + uint32_t bit_padding_1 : 3 = 0; + uint32_t bbm_clks : 4 = 0; + uint32_t bit_padding_2 : 4 = 0; + // 00 = 150*C, 01 = 143*C, 10 = 136*C, 11 = 120*C + uint32_t ot_select : 2 = 0; + uint32_t drv_strength : 2 = 0; + uint32_t filt_isense : 2 = 0; +}; + /** * This register sets the control current for voltage PWM mode stealth chop. */ @@ -544,6 +559,7 @@ struct TMC2160RegisterMap { StealthChop pwmconf = {}; DriveStatus drvstatus = {}; GStatus gstat = {}; + DriveConf drvconf = {}; GlobalScaler glob_scale = {}; }; diff --git a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp index bc8574cee..3958b917c 100644 --- a/include/motor-control/core/stepper_motor/tmc2160_driver.hpp +++ b/include/motor-control/core/stepper_motor/tmc2160_driver.hpp @@ -53,10 +53,11 @@ class TMC2160 { auto operator=(const TMC2160&& c) = delete; ~TMC2160() = default; - auto read(Registers addr, uint32_t command_data, uint32_t message_index) - -> void { + auto read(Registers addr, uint32_t command_data, uint32_t message_index, + uint8_t tags = 0) -> void { auto converted_addr = static_cast<uint8_t>(addr); - _spi_manager.read(converted_addr, command_data, _task_queue, _cs_intf, + uint32_t token = spi::utils::build_token(converted_addr, tags); + _spi_manager.read(token, command_data, _task_queue, _cs_intf, message_index); } @@ -105,6 +106,9 @@ class TMC2160 { if (!set_glob_scaler(_registers.glob_scale)) { return false; } + if (!set_drv_conf(_registers.drvconf)) { + return false; + } _initialized = true; return true; } @@ -272,6 +276,22 @@ class TMC2160 { return false; } + /** + * @brief Update DRV_CONF register + * @param reg New configuration register to set + * @param policy Instance of abstraction policy to use + * @return True if new register was set succesfully, false otherwise + */ + auto set_drv_conf(DriveConf reg) -> bool { + reg.bit_padding_1 = 0; + reg.bit_padding_2 = 0; + if (set_register(reg)) { + _registers.drvconf = reg; + return true; + } + return false; + } + /** * @brief Update GlobalScaler register * @param reg New configuration register to set 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 ce55d848c..d9593d395 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 @@ -59,12 +59,24 @@ class MotorDriverMessageHandler { auto data = driver.handle_spi_read( tmc2160::registers::Registers(static_cast<uint8_t>(m.id.token)), m.rxBuffer); - can::messages::ReadMotorDriverRegisterResponse response_msg{ - .message_index = m.id.message_index, - .reg_address = static_cast<uint8_t>(m.id.token), - .data = data, - }; - can_client.send_can_message(can::ids::NodeId::host, response_msg); + if (spi::utils::tag_in_token( + m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) { + can::messages::ReadMotorDriverErrorStatusResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast<uint8_t>(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } else { + can::messages::ReadMotorDriverRegisterResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast<uint8_t>(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } } } @@ -87,6 +99,15 @@ class MotorDriverMessageHandler { } } + void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) { + LOG("Received read motor driver error register request"); + 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); + } + void handle(const can::messages::GearWriteMotorCurrentRequest& m) { LOG("Received gear write motor current request: hold_current=%d, " "run_current=%d", diff --git a/include/motor-control/core/tasks/messages.hpp b/include/motor-control/core/tasks/messages.hpp index 759a28308..dc3203f7f 100644 --- a/include/motor-control/core/tasks/messages.hpp +++ b/include/motor-control/core/tasks/messages.hpp @@ -6,6 +6,11 @@ namespace motor_control_task_messages { +struct RouteMotorDriverInterrupt { + uint32_t message_index; + uint8_t debounce_count; +}; + #ifdef USE_SENSOR_MOVE using MotionControlTaskMessage = std::variant< std::monostate, can::messages::AddLinearMoveRequest, @@ -16,7 +21,7 @@ using MotionControlTaskMessage = std::variant< can::messages::HomeRequest, can::messages::UpdateMotorPositionEstimationRequest, can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest, - can::messages::AddSensorMoveRequest>; + RouteMotorDriverInterrupt, can::messages::AddSensorMoveRequest>; using MoveGroupTaskMessage = std::variant<std::monostate, can::messages::AddLinearMoveRequest, @@ -34,7 +39,8 @@ using MotionControlTaskMessage = std::variant< can::messages::MotorPositionRequest, can::messages::ReadLimitSwitchRequest, can::messages::HomeRequest, can::messages::UpdateMotorPositionEstimationRequest, - can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest>; + can::messages::GetMotorUsageRequest, can::messages::MotorStatusRequest, + RouteMotorDriverInterrupt>; using MoveGroupTaskMessage = std::variant<std::monostate, can::messages::AddLinearMoveRequest, @@ -47,7 +53,8 @@ using MoveGroupTaskMessage = using MotorDriverTaskMessage = std::variant<std::monostate, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using MoveStatusReporterTaskMessage = std::variant< std::monostate, motor_messages::Ack, motor_messages::UpdatePositionResponse, diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp index f8838703b..74f6e6ca7 100644 --- a/include/motor-control/core/tasks/motion_controller_task.hpp +++ b/include/motor-control/core/tasks/motion_controller_task.hpp @@ -8,26 +8,43 @@ #include "motor-control/core/linear_motion_system.hpp" #include "motor-control/core/stepper_motor/motion_controller.hpp" #include "motor-control/core/tasks/messages.hpp" +#include "motor-control/core/tasks/tmc_motor_driver_common.hpp" namespace motion_controller_task { using TaskMessage = motor_control_task_messages::MotionControlTaskMessage; +/** + * Concept describing a class that can message this task. + * @tparam Client + */ +template <typename Client> +concept TaskClient = requires(Client client, const TaskMessage& m) { + {client.send_motion_controller_queue(m)}; +}; + /** * The message queue message handler. */ template <lms::MotorMechanicalConfig MEConfig, can::message_writer_task::TaskClient CanClient, - usage_storage_task::TaskClient UsageClient> + usage_storage_task::TaskClient UsageClient, + tmc::tasks::TaskClient DriverClient, TaskClient MotionClient> class MotionControllerMessageHandler { public: using MotorControllerType = motion_controller::MotionController<MEConfig>; MotionControllerMessageHandler(MotorControllerType& controller, CanClient& can_client, - UsageClient& usage_client) + UsageClient& usage_client, + DriverClient& driver_client, + MotionClient& motion_client, + std::atomic<bool>& diag0_debounced) : controller{controller}, can_client{can_client}, - usage_client{usage_client} {} + usage_client{usage_client}, + driver_client{driver_client}, + motion_client{motion_client}, + diag0_debounced{diag0_debounced} {} MotionControllerMessageHandler(const MotionControllerMessageHandler& c) = delete; MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) = @@ -52,9 +69,19 @@ class MotionControllerMessageHandler { void handle(const can::messages::EnableMotorRequest& m) { LOG("Received enable motor request"); - controller.enable_motor(); - can_client.send_can_message(can::ids::NodeId::host, - can::messages::ack_from_request(m)); + if (controller.read_tmc_diag0()) { + 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}); + } else { + controller.enable_motor(); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::ack_from_request(m)); + } } void handle(const can::messages::DisableMotorRequest& m) { @@ -92,7 +119,17 @@ class MotionControllerMessageHandler { "groupid=%d, seqid=%d, duration=%d, stopcondition=%d", m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration, m.request_stop_condition); - controller.move(m); + if (controller.read_tmc_diag0()) { + 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}); + } else { + controller.move(m); + } } #ifdef USE_SENSOR_MOVE @@ -109,7 +146,17 @@ class MotionControllerMessageHandler { LOG("Motion Controller Received home request: velocity=%d, " "groupid=%d, seqid=%d\n", m.velocity, m.group_id, m.seq_id); - controller.move(m); + if (controller.read_tmc_diag0()) { + 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}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) { @@ -146,10 +193,47 @@ class MotionControllerMessageHandler { can_client.send_can_message(can::ids::NodeId::host, response); } } + void handle(const can::messages::GetMotorUsageRequest& m) { controller.send_usage_data(m.message_index, usage_client); } + void handle( + const motor_control_task_messages::RouteMotorDriverInterrupt& m) { + if (m.debounce_count > DIAG0_DEBOUNCE_REPS) { + if (controller.read_tmc_diag0()) { + 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}); + driver_client.send_motor_driver_queue( + can::messages::ReadMotorDriverErrorStatusRequest{ + .message_index = m.message_index}); + } + } + diag0_debounced = false; + } else { + vTaskDelay(pdMS_TO_TICKS(DIAG0_DEBOUNCE_DELAY)); + motion_client.send_motion_controller_queue( + increment_message_debounce_count(m)); + } + } + + auto increment_message_debounce_count( + const motor_control_task_messages::RouteMotorDriverInterrupt& m) + -> motor_control_task_messages::RouteMotorDriverInterrupt { + return motor_control_task_messages::RouteMotorDriverInterrupt{ + .message_index = m.message_index, + .debounce_count = static_cast<uint8_t>(m.debounce_count + 1)}; + } + void handle(const can::messages::MotorStatusRequest& m) { auto response = static_cast<uint8_t>(controller.is_motor_enabled()); can::messages::MotorStatusResponse msg{.message_index = m.message_index, @@ -160,6 +244,9 @@ class MotionControllerMessageHandler { MotorControllerType& controller; CanClient& can_client; UsageClient& usage_client; + DriverClient& driver_client; + MotionClient& motion_client; + std::atomic<bool>& diag0_debounced; }; /** @@ -183,12 +270,15 @@ class MotionControllerTask { */ template <lms::MotorMechanicalConfig MEConfig, can::message_writer_task::TaskClient CanClient, - usage_storage_task::TaskClient UsageClient> + usage_storage_task::TaskClient UsageClient, + tmc::tasks::TaskClient DriverClient, TaskClient MotionClient> [[noreturn]] void operator()( motion_controller::MotionController<MEConfig>* controller, - CanClient* can_client, UsageClient* usage_client) { - auto handler = MotionControllerMessageHandler{*controller, *can_client, - *usage_client}; + CanClient* can_client, UsageClient* usage_client, + DriverClient* driver_client, MotionClient* motion_client) { + auto handler = MotionControllerMessageHandler{ + *controller, *can_client, *usage_client, + *driver_client, *motion_client, diag0_debounced}; TaskMessage message{}; bool first_run = true; for (;;) { @@ -204,17 +294,18 @@ class MotionControllerTask { [[nodiscard]] auto get_queue() const -> QueueType& { return queue; } + void run_diag0_interrupt() { + if (!diag0_debounced) { + static_cast<void>(queue.try_write_isr( + motor_control_task_messages::RouteMotorDriverInterrupt{ + .message_index = 0, .debounce_count = 0})); + diag0_debounced = true; + } + } + private: QueueType& queue; -}; - -/** - * Concept describing a class that can message this task. - * @tparam Client - */ -template <typename Client> -concept TaskClient = requires(Client client, const TaskMessage& m) { - {client.send_motion_controller_queue(m)}; + std::atomic<bool> diag0_debounced = false; }; } // namespace motion_controller_task 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 82cff0e01..526b0ebaf 100644 --- a/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/tmc2130_motor_driver_task.hpp @@ -58,12 +58,24 @@ class MotorDriverMessageHandler { auto data = driver.handle_spi_read( tmc2130::registers::Registers(static_cast<uint8_t>(m.id.token)), m.rxBuffer); - can::messages::ReadMotorDriverRegisterResponse response_msg{ - .message_index = m.id.message_index, - .reg_address = static_cast<uint8_t>(m.id.token), - .data = data, - }; - can_client.send_can_message(can::ids::NodeId::host, response_msg); + if (spi::utils::tag_in_token( + m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) { + can::messages::ReadMotorDriverErrorStatusResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast<uint8_t>(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } else { + can::messages::ReadMotorDriverRegisterResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast<uint8_t>(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } } } @@ -86,6 +98,15 @@ class MotorDriverMessageHandler { } } + void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) { + LOG("Received read motor driver error register request"); + 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); + } + void handle(const can::messages::WriteMotorCurrentRequest& m) { LOG("Received write motor current request: hold_current=%d, " "run_current=%d", @@ -154,6 +175,7 @@ class MotorDriverTask { template <typename Client> concept TaskClient = requires(Client client, const TaskMessage& m) { {client.send_motor_driver_queue(m)}; + {client.send_motor_driver_queue_isr(m)}; }; } // namespace tasks 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 9f4e69179..aec315ea7 100644 --- a/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp +++ b/include/motor-control/core/tasks/tmc2160_motor_driver_task.hpp @@ -10,6 +10,7 @@ #include "motor-control/core/tasks/messages.hpp" #include "motor-control/core/tasks/tmc_motor_driver_common.hpp" #include "spi/core/messages.hpp" +#include "spi/core/utils.hpp" namespace tmc2160 { @@ -57,12 +58,24 @@ class MotorDriverMessageHandler { auto data = driver.handle_spi_read( tmc2160::registers::Registers(static_cast<uint8_t>(m.id.token)), m.rxBuffer); - can::messages::ReadMotorDriverRegisterResponse response_msg{ - .message_index = m.id.message_index, - .reg_address = static_cast<uint8_t>(m.id.token), - .data = data, - }; - can_client.send_can_message(can::ids::NodeId::host, response_msg); + if (spi::utils::tag_in_token( + m.id.token, spi::utils::ResponseTag::IS_ERROR_RESPONSE)) { + can::messages::ReadMotorDriverErrorStatusResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast<uint8_t>(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } else { + can::messages::ReadMotorDriverRegisterResponse response_msg{ + .message_index = m.id.message_index, + .reg_address = static_cast<uint8_t>(m.id.token), + .data = data, + }; + can_client.send_can_message(can::ids::NodeId::host, + response_msg); + } } } @@ -85,6 +98,15 @@ class MotorDriverMessageHandler { } } + void handle(const can::messages::ReadMotorDriverErrorStatusRequest& m) { + LOG("Received read motor driver error register request"); + 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); + } + void handle(const can::messages::WriteMotorCurrentRequest& m) { LOG("Received write motor current request: hold_current=%d, " "run_current=%d", @@ -146,6 +168,16 @@ class MotorDriverTask { QueueType& queue; }; +/** + * Concept describing a class that can message this task. + * @tparam Client + */ +template <typename Client> +concept TaskClient = requires(Client client, const TaskMessage& m) { + {client.send_motor_driver_queue(m)}; + {client.send_motor_driver_queue_isr(m)}; +}; + } // namespace tasks } // namespace tmc2160 diff --git a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp index 191373466..0b0839348 100644 --- a/include/motor-control/core/tasks/tmc_motor_driver_common.hpp +++ b/include/motor-control/core/tasks/tmc_motor_driver_common.hpp @@ -8,11 +8,14 @@ namespace tmc { namespace tasks { using SpiResponseMessage = std::tuple<spi::messages::TransactResponse>; -using CanMessageTuple = std::tuple<can::messages::ReadMotorDriverRegister, - can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; +using CanMessageTuple = + std::tuple<can::messages::ReadMotorDriverRegister, + can::messages::WriteMotorDriverRegister, + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using GearCanMessageTuple = std::tuple<can::messages::GearReadMotorDriverRegister, + can::messages::ReadMotorDriverErrorStatusRequest, can::messages::GearWriteMotorDriverRegister, can::messages::GearWriteMotorCurrentRequest>; using CanMessage = @@ -37,6 +40,7 @@ using GearTaskMessage = typename ::utils::VariantCat< template <typename Client> concept TaskClient = requires(Client client, const TaskMessage& m) { {client.send_motor_driver_queue(m)}; + {client.send_motor_driver_queue_isr(m)}; }; /** @@ -46,6 +50,7 @@ concept TaskClient = requires(Client client, const TaskMessage& m) { template <typename Client> concept GearTaskClient = requires(Client client, const GearTaskMessage& m) { {client.send_motor_driver_queue(m)}; + {client.send_motor_driver_queue_isr(m)}; }; }; // namespace tasks }; // namespace tmc 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 986c9fe1d..3049d8f41 100644 --- a/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp +++ b/include/motor-control/firmware/brushed_motor/brushed_motor_hardware.hpp @@ -62,6 +62,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { void read_limit_switch() final; void read_estop_in() final; void read_sync_in() final; + auto read_tmc_diag0() -> bool final; void grip() final; void ungrip() final; void stop_pwm() final; @@ -78,10 +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() -> bool final { - return cancel_request.exchange(false); + auto get_cancel_request() -> CancelRequest final { + CancelRequest exchange_request = {}; + return cancel_request.exchange(exchange_request); + } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + CancelRequest clear_request = {}; + cancel_request.store(clear_request); } - void request_cancel() final { cancel_request.store(true); } auto get_usage_eeprom_config() -> const UsageEEpromConfig& final { return eeprom_config; } @@ -101,7 +113,7 @@ class BrushedMotorHardware : public BrushedMotorHardwareIface { int32_t motor_encoder_overflow_count = 0; ot_utils::pid::PID controller_loop; std::atomic<ControlDirection> control_dir = ControlDirection::unset; - std::atomic<bool> cancel_request = false; + std::atomic<CancelRequest> 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 e750a4f02..ca7be4974 100644 --- a/include/motor-control/firmware/stepper_motor/motor_hardware.hpp +++ b/include/motor-control/firmware/stepper_motor/motor_hardware.hpp @@ -18,6 +18,7 @@ struct HardwareConfig { gpio::PinConfig led; gpio::PinConfig sync_in; gpio::PinConfig estop_in; + gpio::PinConfig diag0; std::optional<gpio::PinConfig> ebrake = std::nullopt; }; @@ -50,16 +51,28 @@ class MotorHardware : public StepperMotorHardwareIface { void read_limit_switch() final; void read_estop_in() final; void read_sync_in() final; + auto read_tmc_diag0() -> bool final; void set_LED(bool status) final; auto get_encoder_pulses() -> int32_t final; void reset_encoder_pulses() final; - auto has_cancel_request() -> bool final { - return cancel_request.exchange(false); - } void disable_encoder() final; void enable_encoder() final; - void request_cancel() final { cancel_request.store(true); } + auto get_cancel_request() -> CancelRequest final { + CancelRequest exchange_request = {}; + return cancel_request.exchange(exchange_request); + } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + CancelRequest clear_request = {}; + cancel_request.store(clear_request); + } auto get_usage_eeprom_config() -> const UsageEEpromConfig& final { return eeprom_config; @@ -67,6 +80,8 @@ class MotorHardware : public StepperMotorHardwareIface { // downward interface - call from timer overflow handler void encoder_overflow(int32_t direction); + auto get_pins() -> HardwareConfig { return pins; } + private: debouncer::Debouncer estop = debouncer::Debouncer{}; debouncer::Debouncer limit = debouncer::Debouncer{}; @@ -77,7 +92,7 @@ class MotorHardware : public StepperMotorHardwareIface { void* enc_handle; const UsageEEpromConfig& eeprom_config; std::atomic<int32_t> motor_encoder_overflow_count = 0; - std::atomic<bool> cancel_request = false; + std::atomic<CancelRequest> cancel_request = {}; static constexpr uint32_t ENCODER_OVERFLOW_PULSES_BIT = 0x1 << 31; }; diff --git a/include/motor-control/simulation/motor_interrupt_driver.hpp b/include/motor-control/simulation/motor_interrupt_driver.hpp index f7dbb193d..6fef39218 100644 --- a/include/motor-control/simulation/motor_interrupt_driver.hpp +++ b/include/motor-control/simulation/motor_interrupt_driver.hpp @@ -9,7 +9,8 @@ namespace motor_interrupt_driver { -template <class StatusClient, typename MotorMoveMessage, class MotorHardware> +template <class StatusClient, class DriverClient, typename MotorMoveMessage, + class MotorHardware> class MotorInterruptDriver { using InterruptQueue = freertos_message_queue::FreeRTOSMessageQueue<MotorMoveMessage>; @@ -18,7 +19,7 @@ class MotorInterruptDriver { can::messages::UpdateMotorPositionEstimationRequest>; using InterruptHandler = motor_handler::MotorInterruptHandler< freertos_message_queue::FreeRTOSMessageQueue, StatusClient, - MotorMoveMessage, MotorHardware>; + DriverClient, MotorMoveMessage, MotorHardware>; public: MotorInterruptDriver(InterruptQueue& q, InterruptHandler& h, diff --git a/include/motor-control/simulation/sim_motor_hardware_iface.hpp b/include/motor-control/simulation/sim_motor_hardware_iface.hpp index 2a8dd67cc..01f45fb21 100644 --- a/include/motor-control/simulation/sim_motor_hardware_iface.hpp +++ b/include/motor-control/simulation/sim_motor_hardware_iface.hpp @@ -51,6 +51,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface { void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + bool read_tmc_diag0() final { return false; } void set_LED(bool) final {} void trigger_limit_switch() { limit_switch_status = true; } bool check_sync_in() final { @@ -87,10 +88,21 @@ 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() -> bool final { - return cancel_request.exchange(false); + auto get_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); + } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request; + cancel_request.store(clear_request); } - void request_cancel() final { cancel_request.store(true); } auto get_usage_eeprom_config() -> motor_hardware::UsageEEpromConfig & final { return eeprom_config; @@ -106,7 +118,7 @@ class SimMotorHardwareIface : public motor_hardware::StepperMotorHardwareIface { Direction _direction = Direction::POSITIVE; float _encoder_ticks_per_pulse = 0; bool estop_detected = false; - std::atomic<bool> cancel_request = false; + std::atomic<motor_hardware::CancelRequest> cancel_request = {}; motor_hardware::UsageEEpromConfig eeprom_config = { std::array<UsageRequestSet, 1>{UsageRequestSet{ .eeprom_key = 0, @@ -137,6 +149,7 @@ class SimBrushedMotorHardwareIface void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + bool read_tmc_diag0() final { return false; } void trigger_limit_switch() { limit_switch_status = true; } void grip() final {} void ungrip() final {} @@ -177,11 +190,21 @@ class SimBrushedMotorHardwareIface return ret; } - auto has_cancel_request() -> bool final { - return cancel_request.exchange(false); + auto get_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); + } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request; + cancel_request.store(clear_request); } - void request_cancel() final { cancel_request.store(true); } - void disable_encoder() final {} void enable_encoder() final {} @@ -201,7 +224,7 @@ class SimBrushedMotorHardwareIface StateManagerHandle _state_manager = nullptr; MoveMessageHardware _id; bool estop_detected = false; - std::atomic<bool> cancel_request = false; + std::atomic<motor_hardware::CancelRequest> cancel_request = {}; BrushedMotorState motor_state = BrushedMotorState::UNHOMED; motor_hardware::UsageEEpromConfig eeprom_config{ std::array<UsageRequestSet, 1>{UsageRequestSet{ @@ -235,6 +258,7 @@ class SimGearMotorHardwareIface void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + bool read_tmc_diag0() final { return false; } void set_LED(bool) final {} void trigger_limit_switch() { limit_switch_status = true; } bool check_sync_in() final { @@ -267,10 +291,21 @@ class SimGearMotorHardwareIface bool check_estop_in() final { return estop_detected; } void set_estop(bool estop_pressed) { estop_detected = estop_pressed; } - auto has_cancel_request() -> bool final { - return cancel_request.exchange(false); + auto get_cancel_request() -> motor_hardware::CancelRequest final { + motor_hardware::CancelRequest exchange_request; + return cancel_request.exchange(exchange_request); + } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request.store(update_request); + } + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request; + cancel_request.store(clear_request); } - void request_cancel() final { cancel_request.store(true); } auto get_usage_eeprom_config() -> motor_hardware::UsageEEpromConfig & final { @@ -287,7 +322,7 @@ class SimGearMotorHardwareIface Direction _direction = Direction::POSITIVE; float _encoder_ticks_per_pulse = 0; bool estop_detected = false; - std::atomic<bool> cancel_request = false; + std::atomic<motor_hardware::CancelRequest> cancel_request = {}; motor_hardware::UsageEEpromConfig eeprom_config = { std::array<UsageRequestSet, 1>{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 410cc8632..ecfdd9b1d 100644 --- a/include/motor-control/tests/mock_brushed_motor_components.hpp +++ b/include/motor-control/tests/mock_brushed_motor_components.hpp @@ -42,6 +42,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { void stop_pwm() final { move_dir = PWM_DIRECTION::unset; } auto check_sync_in() -> bool final { return sync_val; } void read_sync_in() final {} + bool read_tmc_diag0() final { return diag0_val; } auto get_encoder_pulses() -> int32_t final { return (motor_encoder_overflow_count << 16) + enc_val; @@ -71,12 +72,23 @@ 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() -> bool final { - bool old_request = cancel_request; - cancel_request = false; + auto get_cancel_request() -> CancelRequest final { + CancelRequest old_request = cancel_request; + CancelRequest exchange_request{}; + cancel_request = exchange_request; return old_request; } - void request_cancel() final { cancel_request = true; } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request = update_request; + } + void clear_cancel_request() final { + CancelRequest clear_request{}; + cancel_request = clear_request; + } void set_timer_interrupt_running(bool is_running) { timer_interrupt_running = is_running; } @@ -97,6 +109,7 @@ class MockBrushedMotorHardware : public BrushedMotorHardwareIface { int32_t motor_encoder_overflow_count = 0; bool ls_val = false; bool sync_val = false; + bool diag0_val = false; bool estop_in_val = false; bool is_gripping = false; bool motor_enabled = false; @@ -108,7 +121,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}; - bool cancel_request = false; + CancelRequest cancel_request = {}; bool timer_interrupt_running = true; motor_hardware::UsageEEpromConfig eeprom_config = motor_hardware::UsageEEpromConfig{std::array<UsageRequestSet, 3>{ diff --git a/include/motor-control/tests/mock_motor_driver_client.hpp b/include/motor-control/tests/mock_motor_driver_client.hpp new file mode 100644 index 000000000..b4fb715ca --- /dev/null +++ b/include/motor-control/tests/mock_motor_driver_client.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include <vector> + +#include "motor-control/core/tasks/tmc_motor_driver_common.hpp" + +namespace test_mocks { + +struct MockMotorDriverClient { + void send_motor_driver_queue(const tmc::tasks::TaskMessage& m) { + messages.push_back(m); + } + + void send_motor_driver_queue_isr(const tmc::tasks::TaskMessage& m) { + messages.push_back(m); + } + + std::vector<tmc::tasks::TaskMessage> messages{}; +}; + +} // namespace test_mocks diff --git a/include/motor-control/tests/mock_motor_hardware.hpp b/include/motor-control/tests/mock_motor_hardware.hpp index ff6e0bd69..fc04f03b2 100644 --- a/include/motor-control/tests/mock_motor_hardware.hpp +++ b/include/motor-control/tests/mock_motor_hardware.hpp @@ -30,6 +30,7 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface { void read_limit_switch() final {} void read_estop_in() final {} void read_sync_in() final {} + bool read_tmc_diag0() final { return mock_diag0_value; } void set_LED(bool) final {} void set_mock_lim_sw(bool value) { mock_lim_sw_value = value; } void set_mock_estop_in(bool value) { mock_estop_in_value = value; } @@ -39,12 +40,23 @@ 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() -> bool final { - bool old_request = cancel_request; - cancel_request = false; + auto get_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() final { cancel_request = true; } + void set_cancel_request(can::ids::ErrorSeverity error_severity, + can::ids::ErrorCode error_code) final { + motor_hardware::CancelRequest update_request{ + .severity = static_cast<uint8_t>(error_severity), + .code = static_cast<uint8_t>(error_code)}; + cancel_request = update_request; + } + void clear_cancel_request() final { + motor_hardware::CancelRequest clear_request{}; + cancel_request = clear_request; + } void sim_set_timer_interrupt_running(bool is_running) { mock_timer_interrupt_running = is_running; } @@ -61,11 +73,12 @@ class MockMotorHardware : public motor_hardware::StepperMotorHardwareIface { bool mock_lim_sw_value = false; bool mock_estop_in_value = false; bool mock_sync_value = false; + bool mock_diag0_value = false; bool mock_sr_value = false; bool mock_dir_value = false; uint8_t finished_move_id = 0x0; int32_t test_pulses = 0x0; - bool cancel_request = false; + motor_hardware::CancelRequest cancel_request = {}; bool mock_timer_interrupt_running = true; motor_hardware::UsageEEpromConfig eeprom_config = motor_hardware::UsageEEpromConfig{ diff --git a/include/pipettes/core/dispatch_builder.hpp b/include/pipettes/core/dispatch_builder.hpp index 7bb94358b..c02cacd46 100644 --- a/include/pipettes/core/dispatch_builder.hpp +++ b/include/pipettes/core/dispatch_builder.hpp @@ -23,19 +23,22 @@ using TMC2130MotorDispatchTarget = can::dispatch::DispatchParseTarget< linear_motor_tasks::tmc2130_driver::QueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using TMC2160MotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::MotorHandler< linear_motor_tasks::tmc2160_driver::QueueClient>, can::messages::ReadMotorDriverRegister, can::messages::WriteMotorDriverRegister, - can::messages::WriteMotorCurrentRequest>; + can::messages::WriteMotorCurrentRequest, + can::messages::ReadMotorDriverErrorStatusRequest>; using GearMotorDispatchTarget = can::dispatch::DispatchParseTarget< can::message_handlers::motor::GearMotorHandler< gear_motor_tasks::QueueClient>, can::messages::GearReadMotorDriverRegister, + can::messages::ReadMotorDriverErrorStatusRequest, can::messages::GearWriteMotorDriverRegister, can::messages::GearWriteMotorCurrentRequest>; diff --git a/include/pipettes/core/gear_motor_tasks.hpp b/include/pipettes/core/gear_motor_tasks.hpp index 292f73b4d..6f4edc169 100644 --- a/include/pipettes/core/gear_motor_tasks.hpp +++ b/include/pipettes/core/gear_motor_tasks.hpp @@ -31,7 +31,7 @@ using CanWriterTask = can::message_writer_task::MessageWriterTask< using SPIWriterClient = spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>; -void start_tasks( +auto start_tasks( CanWriterTask& can_writer, interfaces::gear_motor::GearMotionControl& motion_controllers, SPIWriterClient& spi_writer, @@ -39,7 +39,10 @@ void start_tasks( can::ids::NodeId id, interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks, eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& - tail_accessor); + tail_accessor) + -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler>; + +void call_run_diag0_interrupt(); /** * Access to all the gear motion tasks. @@ -73,6 +76,9 @@ struct QueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc2160::tasks::gear::TaskMessage& m); + void send_motor_driver_queue_isr( + const tmc2160::tasks::gear::TaskMessage& m); + void send_move_group_queue( const pipettes::tasks::move_group_task::TaskMessage& m); diff --git a/include/pipettes/core/interfaces.hpp b/include/pipettes/core/interfaces.hpp index 999e5f7ea..abca79a9f 100644 --- a/include/pipettes/core/interfaces.hpp +++ b/include/pipettes/core/interfaces.hpp @@ -36,6 +36,8 @@ struct HighThroughputInterruptQueues { UpdatePositionQueue left_update_queue; }; +extern "C" using diag0_handler = void(*)(); + namespace gear_motor { struct UnavailableGearHardware {}; diff --git a/include/pipettes/core/linear_motor_tasks.hpp b/include/pipettes/core/linear_motor_tasks.hpp index ae20e52d8..c4511567a 100644 --- a/include/pipettes/core/linear_motor_tasks.hpp +++ b/include/pipettes/core/linear_motor_tasks.hpp @@ -12,6 +12,7 @@ #include "motor-control/core/tasks/move_status_reporter_task.hpp" #include "motor-control/core/tasks/tmc2130_motor_driver_task.hpp" #include "motor-control/core/tasks/tmc2160_motor_driver_task.hpp" +#include "pipettes/core/interfaces.hpp" #include "pipettes/core/sensor_tasks.hpp" #include "spi/core/writer.hpp" @@ -31,7 +32,7 @@ using SPIWriterClient = spi::writer::Writer<freertos_message_queue::FreeRTOSMessageQueue>; // single channel/8 channel linear motor tasks -void start_tasks( +auto start_tasks( CanWriterTask& can_writer, motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller, @@ -39,10 +40,10 @@ void start_tasks( tmc2130::configs::TMC2130DriverConfig& linear_driver_configs, can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk, eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& - tail_accessor); + tail_accessor) -> interfaces::diag0_handler; // 96/384 linear motor tasks -void start_tasks( +auto start_tasks( CanWriterTask& can_writer, motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller, @@ -50,7 +51,9 @@ void start_tasks( tmc2160::configs::TMC2160DriverConfig& linear_driver_configs, can::ids::NodeId, motor_hardware_task::MotorHardwareTask& lmh_tsk, eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& - tail_accessor); + tail_accessor) -> interfaces::diag0_handler; + +void call_run_diag0_interrupt(); /** * Access to all the linear motion task queues on the pipette. @@ -134,6 +137,11 @@ struct QueueClient : can::message_writer::MessageWriter { driver_queue->try_write(m); } + void send_motor_driver_queue_isr( + const tmc2130::tasks::TaskMessage& m) const { + static_cast<void>(driver_queue->try_write_isr(m)); + } + freertos_message_queue::FreeRTOSMessageQueue<tmc2130::tasks::TaskMessage>* driver_queue{nullptr}; }; @@ -173,6 +181,11 @@ struct QueueClient : can::message_writer::MessageWriter { driver_queue->try_write(m); } + void send_motor_driver_queue_isr( + const tmc2160::tasks::TaskMessage& m) const { + static_cast<void>(driver_queue->try_write_isr(m)); + } + freertos_message_queue::FreeRTOSMessageQueue<tmc2160::tasks::TaskMessage>* driver_queue{nullptr}; }; diff --git a/include/pipettes/core/tasks.hpp b/include/pipettes/core/tasks.hpp index 2a53bc335..149beb395 100644 --- a/include/pipettes/core/tasks.hpp +++ b/include/pipettes/core/tasks.hpp @@ -49,6 +49,8 @@ struct QueueClient : can::message_writer::MessageWriter { void send_motor_driver_queue(const tmc2130::tasks::TaskMessage& m); + void send_motor_driver_queue_isr(const tmc2130::tasks::TaskMessage& m); + void send_move_group_queue(const move_group_task::TaskMessage& m); void send_move_status_reporter_queue( diff --git a/include/pipettes/core/tasks/messages.hpp b/include/pipettes/core/tasks/messages.hpp index 7dcd6388d..ed827881f 100644 --- a/include/pipettes/core/tasks/messages.hpp +++ b/include/pipettes/core/tasks/messages.hpp @@ -10,6 +10,11 @@ namespace task_messages { namespace motor_control_task_messages { +struct RouteMotorDriverInterrupt { + uint32_t message_index; + uint8_t debounce_count; +}; + using MotionControlTaskMessage = std::variant< std::monostate, can::messages::TipActionRequest, can::messages::GearDisableMotorRequest, @@ -17,7 +22,7 @@ using MotionControlTaskMessage = std::variant< can::messages::GetMotionConstraintsRequest, can::messages::SetMotionConstraints, can::messages::StopRequest, can::messages::ReadLimitSwitchRequest, can::messages::GetMotorUsageRequest, - can::messages::MotorStatusRequest>; + can::messages::MotorStatusRequest, RouteMotorDriverInterrupt>; using MoveStatusReporterTaskMessage = std::variant<std::monostate, motor_messages::GearMotorAck, diff --git a/include/pipettes/core/tasks/motion_controller_task.hpp b/include/pipettes/core/tasks/motion_controller_task.hpp index 71c3b3539..5c9db1062 100644 --- a/include/pipettes/core/tasks/motion_controller_task.hpp +++ b/include/pipettes/core/tasks/motion_controller_task.hpp @@ -7,6 +7,7 @@ #include "common/core/logging.h" #include "motor-control/core/linear_motion_system.hpp" #include "motor-control/core/stepper_motor/motion_controller.hpp" +#include "motor-control/core/tasks/tmc_motor_driver_common.hpp" #include "motor-control/core/tasks/usage_storage_task.hpp" #include "pipettes/core/tasks/messages.hpp" @@ -19,22 +20,38 @@ namespace motion_controller_task { using TaskMessage = pipettes::task_messages::motor_control_task_messages:: MotionControlTaskMessage; +/** + * Concept describing a class that can message this task. + * @tparam Client + */ +template <typename Client> +concept TaskClient = requires(Client client, const TaskMessage& m) { + {client.send_motion_controller_queue(m)}; +}; + /** * The message queue message handler. */ template <lms::MotorMechanicalConfig MEConfig, can::message_writer_task::TaskClient CanClient, - usage_storage_task::TaskClient UsageClient> + usage_storage_task::TaskClient UsageClient, + tmc::tasks::GearTaskClient DriverClient, TaskClient MotionClient> class MotionControllerMessageHandler { public: using MotorControllerType = pipette_motion_controller::PipetteMotionController<MEConfig>; MotionControllerMessageHandler(MotorControllerType& controller, CanClient& can_client, - UsageClient& usage_client) + UsageClient& usage_client, + DriverClient& driver_client, + MotionClient& motion_client, + std::atomic<bool>& diag0_debounced) : controller{controller}, can_client{can_client}, - usage_client{usage_client} {} + usage_client{usage_client}, + driver_client{driver_client}, + motion_client{motion_client}, + diag0_debounced{diag0_debounced} {} MotionControllerMessageHandler(const MotionControllerMessageHandler& c) = delete; MotionControllerMessageHandler(const MotionControllerMessageHandler&& c) = @@ -61,9 +78,19 @@ class MotionControllerMessageHandler { LOG("Received enable motor request"); // TODO only toggle the enable pin once since all motors share // a single enable pin line. - controller.enable_motor(); - can_client.send_can_message(can::ids::NodeId::host, - can::messages::ack_from_request(m)); + if (controller.read_tmc_diag0()) { + 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}); + } else { + controller.enable_motor(); + can_client.send_can_message(can::ids::NodeId::host, + can::messages::ack_from_request(m)); + } } void handle(const can::messages::GearDisableMotorRequest& m) { @@ -102,7 +129,17 @@ class MotionControllerMessageHandler { LOG("Motion Controller Received a tip action request: velocity=%d, " "acceleration=%d, groupid=%d, seqid=%d\n", m.velocity, m.acceleration, m.group_id, m.seq_id); - controller.move(m); + if (controller.read_tmc_diag0()) { + 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}); + } else { + controller.move(m); + } } void handle(const can::messages::ReadLimitSwitchRequest& m) { @@ -117,6 +154,44 @@ class MotionControllerMessageHandler { controller.send_usage_data(m.message_index, usage_client); } + void handle(const pipettes::task_messages::motor_control_task_messages:: + RouteMotorDriverInterrupt& m) { + if (m.debounce_count > DIAG0_DEBOUNCE_REPS) { + if (controller.read_tmc_diag0()) { + 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}); + driver_client.send_motor_driver_queue( + can::messages::ReadMotorDriverErrorStatusRequest{ + .message_index = m.message_index}); + } + } + diag0_debounced = false; + } else { + vTaskDelay(pdMS_TO_TICKS(DIAG0_DEBOUNCE_DELAY)); + motion_client.send_motion_controller_queue( + increment_message_debounce_count(m)); + } + } + + auto increment_message_debounce_count( + const pipettes::task_messages::motor_control_task_messages:: + RouteMotorDriverInterrupt& m) -> pipettes::task_messages:: + motor_control_task_messages::RouteMotorDriverInterrupt { + return pipettes::task_messages::motor_control_task_messages:: + RouteMotorDriverInterrupt{ + .message_index = m.message_index, + .debounce_count = static_cast<uint8_t>(m.debounce_count + 1)}; + } + void handle(const can::messages::MotorStatusRequest& m) { auto response = static_cast<uint8_t>(controller.is_motor_enabled()); can::messages::GearMotorStatusResponse msg{ @@ -127,6 +202,9 @@ class MotionControllerMessageHandler { MotorControllerType& controller; CanClient& can_client; UsageClient& usage_client; + DriverClient& driver_client; + MotionClient& motion_client; + std::atomic<bool>& diag0_debounced; }; /** @@ -150,13 +228,16 @@ class MotionControllerTask { */ template <lms::MotorMechanicalConfig MEConfig, can::message_writer_task::TaskClient CanClient, - usage_storage_task::TaskClient UsageClient> + usage_storage_task::TaskClient UsageClient, + tmc::tasks::GearTaskClient DriverClient, TaskClient MotionClient> [[noreturn]] void operator()( pipette_motion_controller::PipetteMotionController<MEConfig>* controller, - CanClient* can_client, UsageClient* usage_client) { - auto handler = MotionControllerMessageHandler{*controller, *can_client, - *usage_client}; + CanClient* can_client, UsageClient* usage_client, + DriverClient* driver_client, MotionClient* motion_client) { + auto handler = MotionControllerMessageHandler{ + *controller, *can_client, *usage_client, + *driver_client, *motion_client, diag0_debounced}; TaskMessage message{}; for (;;) { if (queue.try_read(&message, queue.max_delay)) { @@ -167,17 +248,19 @@ class MotionControllerTask { [[nodiscard]] auto get_queue() const -> QueueType& { return queue; } + void run_diag0_interrupt() { + if (!diag0_debounced) { + static_cast<void>(queue.try_write_isr( + pipettes::task_messages::motor_control_task_messages:: + RouteMotorDriverInterrupt{.message_index = 0, + .debounce_count = 0})); + diag0_debounced = true; + } + } + private: QueueType& queue; -}; - -/** - * Concept describing a class that can message this task. - * @tparam Client - */ -template <typename Client> -concept TaskClient = requires(Client client, const TaskMessage& m) { - {client.send_motion_controller_queue(m)}; + std::atomic<bool> diag0_debounced = false; }; } // namespace motion_controller_task diff --git a/include/pipettes/firmware/interfaces.hpp b/include/pipettes/firmware/interfaces.hpp index 2ff87fcc4..e39fa08fd 100644 --- a/include/pipettes/firmware/interfaces.hpp +++ b/include/pipettes/firmware/interfaces.hpp @@ -20,19 +20,19 @@ namespace interfaces { #ifdef USE_SENSOR_MOVE -template <typename Client> +template <typename Client, typename DriverClient> using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::SensorSyncMove, motor_hardware::MotorHardware>; #else -template <typename Client> +template <typename Client, typename DriverClient> using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, - motor_hardware::MotorHardware>; + freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, + motor_messages::Move, motor_hardware::MotorHardware>; #endif template <typename Client> using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, Client, motor_messages::GearMotorMove, motor_hardware::MotorHardware>; template <PipetteType P> @@ -75,11 +75,15 @@ namespace linear_motor { auto get_interrupt(motor_hardware::MotorHardware& hw, LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>; + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient>; auto get_interrupt(motor_hardware::MotorHardware& hw, HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>; + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient>; auto get_motor_hardware(motor_configs::LowThroughputPipetteMotorHardware pins) -> motor_hardware::MotorHardware; auto get_motor_hardware(motor_configs::HighThroughputPipetteMotorHardware pins) diff --git a/include/pipettes/firmware/interfaces_g4.hpp b/include/pipettes/firmware/interfaces_g4.hpp index ecf9c0813..d50466777 100644 --- a/include/pipettes/firmware/interfaces_g4.hpp +++ b/include/pipettes/firmware/interfaces_g4.hpp @@ -29,19 +29,19 @@ namespace interfaces { #ifdef USE_SENSOR_MOVE -template <typename Client> +template <typename Client, typename DriverClient> using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::SensorSyncMove, motor_hardware::MotorHardware>; #else -template <typename Client> +template <typename Client, typename DriverClient> using MotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, motor_messages::Move, - motor_hardware::MotorHardware>; + freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, + motor_messages::Move, motor_hardware::MotorHardware>; #endif template <typename Client> using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, Client, motor_messages::GearMotorMove, motor_hardware::MotorHardware>; template <PipetteType P> @@ -84,11 +84,15 @@ namespace linear_motor { auto get_interrupt(motor_hardware::MotorHardware& hw, LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>; + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient>; auto get_interrupt(motor_hardware::MotorHardware& hw, HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>; + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient>; auto get_motor_hardware(motor_hardware::HardwareConfig pins) -> motor_hardware::MotorHardware; auto get_motion_control(motor_hardware::MotorHardware& hw, diff --git a/include/pipettes/simulator/interfaces.hpp b/include/pipettes/simulator/interfaces.hpp index 59ee3fbed..e4a9d3446 100644 --- a/include/pipettes/simulator/interfaces.hpp +++ b/include/pipettes/simulator/interfaces.hpp @@ -12,22 +12,22 @@ namespace interfaces { -template <typename Client> +template <typename Client, typename DriverClient> using MotorInterruptHandlerType = #ifdef USE_SENSOR_MOVE motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::SensorSyncMove, sim_motor_hardware_iface::SimMotorHardwareIface>; #else motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, DriverClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>; #endif template <typename Client> using GearMotorInterruptHandlerType = motor_handler::MotorInterruptHandler< - freertos_message_queue::FreeRTOSMessageQueue, Client, + freertos_message_queue::FreeRTOSMessageQueue, Client, Client, motor_messages::GearMotorMove, sim_motor_hardware_iface::SimGearMotorHardwareIface>; @@ -71,38 +71,52 @@ namespace linear_motor { auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>; + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient>; auto get_interrupt(sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient>; + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient>; auto get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, - MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler) -> + MotorInterruptHandlerType<linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient>& + handler) -> #ifdef USE_SENSOR_MOVE motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient, + motor_messages::SensorSyncMove, sim_motor_hardware_iface::SimMotorHardwareIface>; #else motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::Move, + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>; #endif auto get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, - MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler) -> + MotorInterruptHandlerType<linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient>& + handler) -> #ifdef USE_SENSOR_MOVE motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient, + motor_messages::SensorSyncMove, sim_motor_hardware_iface::SimMotorHardwareIface>; #else motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::Move, + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface>; #endif @@ -126,11 +140,13 @@ struct GearInterruptHandlers { struct GearInterruptDrivers { motor_interrupt_driver::MotorInterruptDriver< - gear_motor_tasks::QueueClient, motor_messages::GearMotorMove, + gear_motor_tasks::QueueClient, gear_motor_tasks::QueueClient, + motor_messages::GearMotorMove, sim_motor_hardware_iface::SimGearMotorHardwareIface> left; motor_interrupt_driver::MotorInterruptDriver< - gear_motor_tasks::QueueClient, motor_messages::GearMotorMove, + gear_motor_tasks::QueueClient, gear_motor_tasks::QueueClient, + motor_messages::GearMotorMove, sim_motor_hardware_iface::SimGearMotorHardwareIface> right; }; diff --git a/include/spi/core/utils.hpp b/include/spi/core/utils.hpp index e41b4468f..0f28e89fa 100644 --- a/include/spi/core/utils.hpp +++ b/include/spi/core/utils.hpp @@ -16,6 +16,39 @@ struct ChipSelectInterface { void* GPIO_handle; }; +// Bit positions to pack in an 8 bit response tag +enum class ResponseTag : size_t { + IS_ERROR_RESPONSE = 0, +}; + +[[nodiscard]] constexpr auto byte_from_tag(ResponseTag tag) -> uint8_t { + return (1 << static_cast<size_t>(tag)); +} + +template <std::ranges::range Tags> +[[nodiscard]] auto byte_from_tags(const Tags& tags) -> uint8_t { + uint8_t result = 0; + for (auto tag : tags) { + result |= byte_from_tag(tag); + } + return result; +} + +[[nodiscard]] inline constexpr auto tag_in_token(uint32_t token, + ResponseTag tag) -> bool { + return bool(token & + (1 << (static_cast<size_t>(tag) + static_cast<size_t>(8)))); +} + +[[nodiscard]] inline constexpr auto build_token(uint8_t reg, uint8_t tags = 0) + -> uint32_t { + return (static_cast<uint32_t>(tags) << 8) | (reg); +} + +template <typename RegType> +[[nodiscard]] inline constexpr auto reg_from_token(uint32_t id) -> RegType { + return static_cast<RegType>(static_cast<uint8_t>(id & 0xff)); +} } // namespace utils -} // namespace spi \ No newline at end of file +} // namespace spi diff --git a/include/spi/core/writer.hpp b/include/spi/core/writer.hpp index a3e2e1710..cf887f4b6 100644 --- a/include/spi/core/writer.hpp +++ b/include/spi/core/writer.hpp @@ -46,13 +46,13 @@ class Writer { * @return A success boolean */ template <OriginatingResponseQueue RQType> - auto read(uint8_t register_addr, uint32_t command_data, - RQType& response_queue, utils::ChipSelectInterface cs_intf, - uint32_t message_index) -> bool { - auto txBuffer = build_message(register_addr, spi::hardware::Mode::READ, - command_data); + auto read(uint32_t token, uint32_t command_data, RQType& response_queue, + utils::ChipSelectInterface cs_intf, uint32_t message_index) + -> bool { + auto txBuffer = build_message(utils::reg_from_token<uint8_t>(token), + spi::hardware::Mode::READ, command_data); TransactionIdentifier _transaction_id{ - .token = register_addr, + .token = token, .command_type = static_cast<uint8_t>(spi::hardware::Mode::READ), .requires_response = false, .message_index = message_index}; @@ -83,7 +83,7 @@ class Writer { auto txBuffer = build_message(register_addr, spi::hardware::Mode::WRITE, command_data); TransactionIdentifier _transaction_id{ - .token = register_addr, + .token = utils::build_token(register_addr), .command_type = static_cast<uint8_t>(spi::hardware::Mode::WRITE), .requires_response = true}; Transact message{ diff --git a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp index 2b5d1314d..1d7690cfd 100644 --- a/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp +++ b/motor-control/firmware/brushed_motor/brushed_motor_hardware.cpp @@ -58,6 +58,8 @@ void BrushedMotorHardware::read_sync_in() { sync.debounce_update(gpio::is_set(pins.sync_in)); } +bool BrushedMotorHardware::read_tmc_diag0() { return 0; } + int32_t BrushedMotorHardware::get_encoder_pulses() { if (!enc_handle) { return 0; diff --git a/motor-control/firmware/stepper_motor/motor_hardware.cpp b/motor-control/firmware/stepper_motor/motor_hardware.cpp index 76c64faa1..7cee57b34 100644 --- a/motor-control/firmware/stepper_motor/motor_hardware.cpp +++ b/motor-control/firmware/stepper_motor/motor_hardware.cpp @@ -50,6 +50,8 @@ void MotorHardware::read_estop_in() { void MotorHardware::read_sync_in() { sync = gpio::is_set(pins.sync_in); } +bool MotorHardware::read_tmc_diag0() { return gpio::is_set(pins.diag0); } + void MotorHardware::set_LED(bool status) { if (status) { gpio::set(pins.led); diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 578754c46..9f1e9e7a7 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -523,7 +523,9 @@ SCENARIO("handler recovers from error state") { test_objs.handler.error_handled = true; WHEN("a cancel request is received") { - test_objs.hw.request_cancel(); + test_objs.hw.set_cancel_request( + 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_limit_switch.cpp b/motor-control/tests/test_limit_switch.cpp index 8dbd107a2..962874bee 100644 --- a/motor-control/tests/test_limit_switch.cpp +++ b/motor-control/tests/test_limit_switch.cpp @@ -2,6 +2,7 @@ #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/motor_messages.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -16,11 +17,12 @@ struct HandlerContainer { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue{}; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; stall_check::StallCheck stall{10, 10, 10}; - MotorInterruptHandler<test_mocks::MockMessageQueue, - test_mocks::MockMoveStatusReporterClient, Move, - test_mocks::MockMotorHardware> - handler{queue, reporter, hw, stall, update_position_queue}; + MotorInterruptHandler< + test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient, + test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware> + handler{queue, reporter, driver, hw, stall, update_position_queue}; }; static constexpr sq0_31 default_velocity = diff --git a/motor-control/tests/test_limit_switch_backoff.cpp b/motor-control/tests/test_limit_switch_backoff.cpp index c4967c890..df1e462a1 100644 --- a/motor-control/tests/test_limit_switch_backoff.cpp +++ b/motor-control/tests/test_limit_switch_backoff.cpp @@ -2,6 +2,7 @@ #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/motor_messages.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -16,11 +17,12 @@ struct HandlerContainer { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue{}; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; stall_check::StallCheck stall{10, 10, 10}; - MotorInterruptHandler<test_mocks::MockMessageQueue, - test_mocks::MockMoveStatusReporterClient, Move, - test_mocks::MockMotorHardware> - handler{queue, reporter, hw, stall, update_position_queue}; + MotorInterruptHandler< + test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient, + test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware> + handler{queue, reporter, driver, hw, stall, update_position_queue}; }; SCENARIO( diff --git a/motor-control/tests/test_motor_interrupt_handler.cpp b/motor-control/tests/test_motor_interrupt_handler.cpp index cb83e8443..3c44db5f4 100644 --- a/motor-control/tests/test_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_motor_interrupt_handler.cpp @@ -3,6 +3,7 @@ #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/motor_messages.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -15,11 +16,13 @@ struct MotorContainer { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue{}; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; stall_check::StallCheck st{1, 1, 10}; MotorInterruptHandler<test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient, + test_mocks::MockMotorDriverClient, motor_messages::Move, test_mocks::MockMotorHardware> - handler{queue, reporter, hw, st, update_position_queue}; + handler{queue, reporter, driver, hw, st, update_position_queue}; }; SCENARIO("a move is cancelled due to a stop request") { @@ -41,7 +44,9 @@ 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(); + test_objs.hw.set_cancel_request( + 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); diff --git a/motor-control/tests/test_motor_interrupt_queue.cpp b/motor-control/tests/test_motor_interrupt_queue.cpp index 9d3de068e..906da0ebd 100644 --- a/motor-control/tests/test_motor_interrupt_queue.cpp +++ b/motor-control/tests/test_motor_interrupt_queue.cpp @@ -2,6 +2,7 @@ #include "catch2/catch.hpp" #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -15,10 +16,11 @@ TEST_CASE("motor interrupt handler queue functionality") { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; test_mocks::MockMotorHardware hardware; stall_check::StallCheck stall(10, 10, 10); - auto handler = MotorInterruptHandler(queue, reporter, hardware, stall, - update_position_queue); + auto handler = MotorInterruptHandler(queue, reporter, driver, hardware, + stall, update_position_queue); WHEN("add multiple moves to the queue") { THEN("all the moves should exist in order") { diff --git a/motor-control/tests/test_motor_pulse.cpp b/motor-control/tests/test_motor_pulse.cpp index 856edaad1..312aa86d2 100644 --- a/motor-control/tests/test_motor_pulse.cpp +++ b/motor-control/tests/test_motor_pulse.cpp @@ -1,6 +1,7 @@ #include "catch2/catch.hpp" #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -21,11 +22,12 @@ struct HandlerContainer { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue{}; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; stall_check::StallCheck stall{tick_per_um, tick_per_um, stall_threshold_um}; - MotorInterruptHandler<test_mocks::MockMessageQueue, - test_mocks::MockMoveStatusReporterClient, Move, - test_mocks::MockMotorHardware> - handler{queue, reporter, hw, stall, update_position_queue}; + MotorInterruptHandler< + test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient, + test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware> + handler{queue, reporter, driver, hw, stall, update_position_queue}; }; sq0_31 convert_velocity(float f) { diff --git a/motor-control/tests/test_motor_stall_handling.cpp b/motor-control/tests/test_motor_stall_handling.cpp index 07ddb5006..f75b2db23 100644 --- a/motor-control/tests/test_motor_stall_handling.cpp +++ b/motor-control/tests/test_motor_stall_handling.cpp @@ -2,6 +2,7 @@ #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" #include "motor-control/core/usage_messages.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -21,11 +22,12 @@ struct HandlerContainer { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue{}; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; stall_check::StallCheck stall{tick_per_um, tick_per_um, stall_threshold_um}; - MotorInterruptHandler<test_mocks::MockMessageQueue, - test_mocks::MockMoveStatusReporterClient, Move, - test_mocks::MockMotorHardware> - handler{queue, reporter, hw, stall, update_position_queue}; + MotorInterruptHandler< + test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient, + test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware> + handler{queue, reporter, driver, hw, stall, update_position_queue}; }; SCENARIO("motor handler stall detection") { diff --git a/motor-control/tests/test_sync_handling.cpp b/motor-control/tests/test_sync_handling.cpp index aa59325a5..f35e21d0c 100644 --- a/motor-control/tests/test_sync_handling.cpp +++ b/motor-control/tests/test_sync_handling.cpp @@ -2,6 +2,7 @@ #include "common/tests/mock_message_queue.hpp" #include "motor-control/core/motor_messages.hpp" #include "motor-control/core/stepper_motor/motor_interrupt_handler.hpp" +#include "motor-control/tests/mock_motor_driver_client.hpp" #include "motor-control/tests/mock_motor_hardware.hpp" #include "motor-control/tests/mock_move_status_reporter_client.hpp" @@ -16,11 +17,12 @@ struct HandlerContainer { can::messages::UpdateMotorPositionEstimationRequest> update_position_queue{}; test_mocks::MockMoveStatusReporterClient reporter{}; + test_mocks::MockMotorDriverClient driver{}; stall_check::StallCheck stall{10, 10, 10}; - MotorInterruptHandler<test_mocks::MockMessageQueue, - test_mocks::MockMoveStatusReporterClient, Move, - test_mocks::MockMotorHardware> - handler{queue, reporter, hw, stall, update_position_queue}; + MotorInterruptHandler< + test_mocks::MockMessageQueue, test_mocks::MockMoveStatusReporterClient, + test_mocks::MockMotorDriverClient, Move, test_mocks::MockMotorHardware> + handler{queue, reporter, driver, hw, stall, update_position_queue}; }; static constexpr sq0_31 default_velocity = diff --git a/pipettes/core/gear_motor_tasks.cpp b/pipettes/core/gear_motor_tasks.cpp index c5caabc5d..43e4d5585 100644 --- a/pipettes/core/gear_motor_tasks.cpp +++ b/pipettes/core/gear_motor_tasks.cpp @@ -37,7 +37,21 @@ static auto move_status_task_builder_right = freertos_task::TaskStarter< static auto left_usage_storage_task_builder = freertos_task::TaskStarter<256, usage_storage_task::UsageStorageTask>{}; -void gear_motor_tasks::start_tasks( +void call_run_diag0_left_interrupt() { + if (gear_motor_tasks::get_left_gear_tasks().motion_controller) { + return gear_motor_tasks::get_left_gear_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + +void call_run_diag0_right_interrupt() { + if (gear_motor_tasks::get_right_gear_tasks().motion_controller) { + return gear_motor_tasks::get_right_gear_tasks() + .motion_controller->run_diag0_interrupt(); + } +} + +auto gear_motor_tasks::start_tasks( gear_motor_tasks::CanWriterTask& can_writer, interfaces::gear_motor::GearMotionControl& motion_controllers, gear_motor_tasks::SPIWriterClient& spi_writer, @@ -45,7 +59,8 @@ void gear_motor_tasks::start_tasks( can::ids::NodeId id, interfaces::gear_motor::GearMotorHardwareTasks& gmh_tsks, eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& - tail_accessor) { + tail_accessor) + -> std::tuple<interfaces::diag0_handler, interfaces::diag0_handler> { left_queue_client.set_node_id(id); right_queue_client.set_node_id(id); @@ -55,9 +70,9 @@ void gear_motor_tasks::start_tasks( auto& right_tasks = gear_motor_tasks::get_right_gear_tasks(); // Left Gear Motor Tasks - auto& motion_left = mc_task_builder_left.start(5, "motion controller", - motion_controllers.left, - left_queues, left_queues); + auto& motion_left = mc_task_builder_left.start( + 5, "motion controller", motion_controllers.left, left_queues, + left_queues, left_queues, left_queues); auto& tmc2160_driver_left = tmc2160_driver_task_builder_left.start( 5, "tmc2160 driver", gear_driver_configs.left_gear_motor, left_queues, spi_writer); @@ -87,7 +102,7 @@ void gear_motor_tasks::start_tasks( // Right Gear Motor Tasks auto& motion_right = mc_task_builder_right.start( 5, "motion controller", motion_controllers.right, right_queues, - right_queues); + right_queues, right_queues, right_queues); auto& tmc2160_driver_right = tmc2160_driver_task_builder_right.start( 5, "tmc2160 driver", gear_driver_configs.right_gear_motor, right_queues, spi_writer); @@ -116,6 +131,9 @@ void gear_motor_tasks::start_tasks( gmh_tsks.left.start_task(); gmh_tsks.right.start_task(); + + return std::make_tuple(call_run_diag0_left_interrupt, + call_run_diag0_right_interrupt); } gear_motor_tasks::QueueClient::QueueClient() @@ -133,6 +151,11 @@ void gear_motor_tasks::QueueClient::send_motor_driver_queue( driver_queue->try_write(m); } +void gear_motor_tasks::QueueClient::send_motor_driver_queue_isr( + const tmc2160::tasks::gear::TaskMessage& m) { + static_cast<void>(driver_queue->try_write_isr(m)); +} + void gear_motor_tasks::QueueClient::send_move_group_queue( const pipettes::tasks::move_group_task::TaskMessage& m) { move_group_queue->try_write(m); diff --git a/pipettes/core/linear_motor_tasks.cpp b/pipettes/core/linear_motor_tasks.cpp index 67909454b..86bd526e3 100644 --- a/pipettes/core/linear_motor_tasks.cpp +++ b/pipettes/core/linear_motor_tasks.cpp @@ -31,7 +31,7 @@ static auto linear_usage_storage_task_builder = static auto eeprom_data_rev_update_builder = freertos_task::TaskStarter<256, eeprom::data_rev_task::UpdateDataRevTask>{}; -void linear_motor_tasks::start_tasks( +auto linear_motor_tasks::start_tasks( linear_motor_tasks::CanWriterTask& can_writer, motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller, @@ -39,7 +39,7 @@ void linear_motor_tasks::start_tasks( tmc2130::configs::TMC2130DriverConfig& linear_driver_configs, can::ids::NodeId id, motor_hardware_task::MotorHardwareTask& lmh_tsk, eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& - tail_accessor) { + tail_accessor) -> interfaces::diag0_handler { tmc2130_queue_client.set_node_id(id); motion_queue_client.set_node_id(id); @@ -49,8 +49,9 @@ void linear_motor_tasks::start_tasks( auto& motion_tasks = linear_motor_tasks::get_tasks(); // Linear Motor Tasks - auto& motion = mc_task_builder.start(5, "motion controller", - motion_controller, queues, queues); + auto& motion = + mc_task_builder.start(5, "motion controller", motion_controller, queues, + queues, tmc2130_queues, queues); auto& tmc2130_driver = tmc2130_driver_task_builder.start( 5, "tmc2130 driver", linear_driver_configs, queues, spi_writer); auto& move_group = @@ -80,9 +81,11 @@ void linear_motor_tasks::start_tasks( queues.move_status_report_queue = &move_status_reporter.get_queue(); queues.usage_storage_queue = &usage_storage_task.get_queue(); lmh_tsk.start_task(); + + return linear_motor_tasks::call_run_diag0_interrupt; } -void linear_motor_tasks::start_tasks( +auto linear_motor_tasks::start_tasks( linear_motor_tasks::CanWriterTask& can_writer, motion_controller::MotionController<lms::LeadScrewConfig>& motion_controller, @@ -90,7 +93,7 @@ void linear_motor_tasks::start_tasks( tmc2160::configs::TMC2160DriverConfig& linear_driver_configs, can::ids::NodeId id, motor_hardware_task::MotorHardwareTask& lmh_tsk, eeprom::dev_data::DevDataTailAccessor<sensor_tasks::QueueClient>& - tail_accessor) { + tail_accessor) -> interfaces::diag0_handler { tmc2160_queue_client.set_node_id(id); motion_queue_client.set_node_id(id); @@ -100,8 +103,9 @@ void linear_motor_tasks::start_tasks( auto& motion_tasks = linear_motor_tasks::get_tasks(); // Linear Motor Tasks - auto& motion = mc_task_builder.start(5, "motion controller", - motion_controller, queues, queues); + auto& motion = + mc_task_builder.start(5, "motion controller", motion_controller, queues, + queues, tmc2160_queues, queues); auto& tmc2160_driver = tmc2160_driver_task_builder.start( 5, "tmc2160 driver", linear_driver_configs, queues, spi_writer); auto& move_group = @@ -133,6 +137,15 @@ void linear_motor_tasks::start_tasks( queues.usage_storage_queue = &usage_storage_task.get_queue(); lmh_tsk.start_task(); + + return linear_motor_tasks::call_run_diag0_interrupt; +} + +void linear_motor_tasks::call_run_diag0_interrupt() { + if (linear_motor_tasks::get_tasks().motion_controller) { + return linear_motor_tasks::get_tasks() + .motion_controller->run_diag0_interrupt(); + } } linear_motor_tasks::QueueClient::QueueClient() diff --git a/pipettes/firmware/hardware_config.c b/pipettes/firmware/hardware_config.c index 336feaad2..b4ada4b98 100644 --- a/pipettes/firmware/hardware_config.c +++ b/pipettes/firmware/hardware_config.c @@ -42,6 +42,9 @@ IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type) { // Tip sense line for the 96 channel (rear) // PC7 -> GPIO_EXTI6 (EXTI9_5_IRQn) + + // Diag0 line for the 96 channel + // PB6 -> GPIO_EXTI6 (EXTI9_5_IRQn) return EXTI9_5_IRQn; case gpio_block_15_10: // Data ready line for 96 channel (front) @@ -49,6 +52,9 @@ IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type) { // Tip sense line for the 96 channel (front) // PC12 -> GPIO_EXTI11 (EXTI15_10_IRQn) + + // Diag0 line for single and eight channel + // PC11 -> GPIO_EXTI11 (EXTI15_10_IRQn) return EXTI15_10_IRQn; case gpio_block_3: // Single channel data ready line @@ -172,8 +178,7 @@ static uint16_t get_motor_driver_pins_lt(GPIO_TypeDef* for_handle) { * Step Pin -> PC7 * Enable Pin -> PA10 * - * VREF (TMC2130) - * PA5 + * VREF (TMC2130) -> PA5 */ switch((uint32_t)for_handle) { case (uint32_t)GPIOA: return GPIO_PIN_10; @@ -219,3 +224,15 @@ uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO return get_motor_driver_pins_lt(for_handle); } } + +uint16_t pipette_hardware_motor_driver_diag0_pin(const PipetteType pipette_type) { + switch (pipette_type) { + case NINETY_SIX_CHANNEL: + case THREE_EIGHTY_FOUR_CHANNEL: + return GPIO_PIN_6; + case SINGLE_CHANNEL: + case EIGHT_CHANNEL: + default: + return GPIO_PIN_11; + } +} diff --git a/pipettes/firmware/hardware_config.h b/pipettes/firmware/hardware_config.h index d203ddb49..730cd48e2 100644 --- a/pipettes/firmware/hardware_config.h +++ b/pipettes/firmware/hardware_config.h @@ -1,4 +1,5 @@ #pragma once +#include <stdbool.h> #include "pipettes/core/pipette_type.h" #include "platform_specific_hal_conf.h" @@ -25,5 +26,6 @@ typedef enum { uint16_t pipette_hardware_spi_pins(const PipetteType pipette_type, GPIO_TypeDef* which_handle); uint16_t pipette_hardware_motor_driver_pins(const PipetteType pipette_type, GPIO_TypeDef* for_handle); +uint16_t pipette_hardware_motor_driver_diag0_pin(const PipetteType pipette_type); PipetteHardwarePin pipette_hardware_get_gpio(const PipetteType pipette_type, PipetteHardwareDevice device); IRQn_Type get_interrupt_line(GPIOInterruptBlock gpio_pin_type); diff --git a/pipettes/firmware/interfaces.cpp b/pipettes/firmware/interfaces.cpp index ff40c65ef..47a68a2cb 100644 --- a/pipettes/firmware/interfaces.cpp +++ b/pipettes/firmware/interfaces.cpp @@ -58,18 +58,24 @@ void linear_motor::encoder_interrupt(motor_hardware::MotorHardware& hw, auto linear_motor::get_interrupt(motor_hardware::MotorHardware& hw, LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> { + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient> { return motor_handler::MotorInterruptHandler( - queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall, + queues.plunger_queue, linear_motor_tasks::get_queues(), + linear_motor_tasks::tmc2130_driver::get_queues(), hw, stall, queues.plunger_update_queue); } auto linear_motor::get_interrupt(motor_hardware::MotorHardware& hw, HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> { + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient> { return motor_handler::MotorInterruptHandler( - queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall, + queues.plunger_queue, linear_motor_tasks::get_queues(), + linear_motor_tasks::tmc2160_driver::get_queues(), hw, stall, queues.plunger_update_queue); } @@ -131,10 +137,12 @@ auto gear_motor::get_interrupts(gear_motor::GearHardware& hw, return gear_motor::GearInterruptHandlers{ .left = motor_handler::MotorInterruptHandler( queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(), - hw.left, stall.left, queues.left_update_queue), + gear_motor_tasks::get_left_gear_queues(), hw.left, stall.left, + queues.left_update_queue), .right = motor_handler::MotorInterruptHandler( queues.right_motor_queue, gear_motor_tasks::get_right_gear_queues(), - hw.right, stall.right, queues.right_update_queue)}; + gear_motor_tasks::get_right_gear_queues(), hw.right, stall.right, + queues.right_update_queue)}; } auto gear_motor::get_interrupts(gear_motor::UnavailableGearHardware&, diff --git a/pipettes/firmware/main.cpp b/pipettes/firmware/main.cpp index 4025951bd..c76ebce94 100644 --- a/pipettes/firmware/main.cpp +++ b/pipettes/firmware/main.cpp @@ -46,6 +46,10 @@ constexpr auto PIPETTE_TYPE = get_pipette_type(); static auto iWatchdog = iwdg::IndependentWatchDog{}; +static interfaces::diag0_handler call_linear_diag0_handler = nullptr; +static interfaces::diag0_handler call_left_gear_diag0_handler = nullptr; +static interfaces::diag0_handler call_right_gear_diag0_handler = nullptr; + static auto can_bus_1 = can::hal::bus::HalCanBus( can_get_device_handle(), utility_configs::led_gpio(PIPETTE_TYPE)); @@ -152,6 +156,22 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { sensor_queue_client.tip_notification_queue_rear->try_write_isr( sensors::tip_presence::TipStatusChangeDetected{})); } + } else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) { + if (call_linear_diag0_handler != nullptr) { + if (*call_linear_diag0_handler != nullptr) { + (*call_linear_diag0_handler)(); + } + } + if (call_left_gear_diag0_handler != nullptr) { + if (*call_left_gear_diag0_handler != nullptr) { + (*call_left_gear_diag0_handler)(); + } + } + if (call_right_gear_diag0_handler != nullptr) { + if (*call_right_gear_diag0_handler != nullptr) { + (*call_right_gear_diag0_handler)(); + } + } } } @@ -196,13 +216,15 @@ auto initialize_motor_tasks( initialize_linear_timer(plunger_callback); initialize_gear_timer(gear_callback_wrapper); initialize_enc_timer(encoder_callback); - linear_motor_tasks::start_tasks( + call_linear_diag0_handler = linear_motor_tasks::start_tasks( *central_tasks::get_tasks().can_writer, linear_motion_control, peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk, tail_accessor); - gear_motor_tasks::start_tasks( - *central_tasks::get_tasks().can_writer, gear_motion, - peripheral_tasks::get_spi_client(), conf, id, gmh_tsks, tail_accessor); + std::tie(call_left_gear_diag0_handler, call_right_gear_diag0_handler) = + gear_motor_tasks::start_tasks(*central_tasks::get_tasks().can_writer, + gear_motion, + peripheral_tasks::get_spi_client(), conf, + id, gmh_tsks, tail_accessor); } auto initialize_motor_tasks( can::ids::NodeId id, @@ -231,7 +253,7 @@ auto initialize_motor_tasks( initialize_linear_timer(plunger_callback); initialize_enc_timer(encoder_callback); - linear_motor_tasks::start_tasks( + call_linear_diag0_handler = linear_motor_tasks::start_tasks( *central_tasks::get_tasks().can_writer, linear_motion_control, peripheral_tasks::get_spi_client(), conf.linear_motor, id, lmh_tsk, tail_accessor); diff --git a/pipettes/firmware/motor_configurations.cpp b/pipettes/firmware/motor_configurations.cpp index 425cd544b..70c96da5e 100644 --- a/pipettes/firmware/motor_configurations.cpp +++ b/pipettes/firmware/motor_configurations.cpp @@ -11,7 +11,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) switch (which) { case TMC2160PipetteAxis::left_gear_motor: return tmc2160::configs::TMC2160DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -24,6 +24,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) .tbl = 0x2, .mres = 0x4}, .coolconf = {.sgt = 0x6}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xa7}}, .current_config = { @@ -37,7 +38,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) }}; case TMC2160PipetteAxis::right_gear_motor: return tmc2160::configs::TMC2160DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -50,6 +51,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) .tbl = 0x2, .mres = 0x4}, .coolconf = {.sgt = 0x6}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xa7}}, .current_config = { @@ -64,7 +66,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) case TMC2160PipetteAxis::linear_motor: default: return tmc2160::configs::TMC2160DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 0}, + .registers = {.gconfig = {.en_pwm_mode = 0, .diag0_error = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -79,6 +81,7 @@ auto motor_configs::driver_config_by_axis(TMC2160PipetteAxis which) .mres = 0x4, .intpol = 0x1}, .coolconf = {.sgt = 0x6}, + .drvconf = {.ot_select = 0}, .glob_scale = {.global_scaler = 0xff}}, .current_config = { @@ -99,7 +102,7 @@ auto motor_configs::driver_config_by_axis(TMC2130PipetteAxis which) case TMC2130PipetteAxis::linear_motor: default: return tmc2130::configs::TMC2130DriverConfig{ - .registers = {.gconfig = {.en_pwm_mode = 1}, + .registers = {.gconfig = {.en_pwm_mode = 1, .diag0_error = 1}, .ihold_irun = {.hold_current = 0x2, .run_current = 0x10, .hold_current_delay = 0x7}, @@ -163,6 +166,11 @@ auto motor_configs::hardware_config_by_axis(TMC2130PipetteAxis which) .port = GPIOC, .pin = GPIO_PIN_12, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_11, + .active_setting = GPIO_PIN_RESET}, }; } } @@ -199,6 +207,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which) .port = GPIOB, .pin = GPIO_PIN_9, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_6, + .active_setting = GPIO_PIN_RESET}, }; case TMC2160PipetteAxis::left_gear_motor: return motor_hardware::HardwareConfig{ @@ -229,6 +242,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which) .port = GPIOB, .pin = GPIO_PIN_9, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_6, + .active_setting = GPIO_PIN_RESET}, }; case TMC2160PipetteAxis::linear_motor: default: @@ -265,6 +283,11 @@ auto motor_configs::hardware_config_by_axis(TMC2160PipetteAxis which) .port = GPIOB, .pin = GPIO_PIN_9, .active_setting = GPIO_PIN_RESET}, + .diag0 = + {// NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOB, + .pin = GPIO_PIN_6, + .active_setting = GPIO_PIN_RESET}, }; } } diff --git a/pipettes/firmware/motor_hardware.c b/pipettes/firmware/motor_hardware.c index 0792f25d5..867a7d4fd 100644 --- a/pipettes/firmware/motor_hardware.c +++ b/pipettes/firmware/motor_hardware.c @@ -94,6 +94,10 @@ void motor_driver_gpio_init() { // Enable Dir/Step pin GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA); HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + // Diag0 pin + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_diag0_pin(pipette_type); + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); } else { // Enable Dir/Step pin GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOA); @@ -101,7 +105,10 @@ void motor_driver_gpio_init() { // Enable/Dir/Step pin GPIO_InitStruct.Pin = pipette_hardware_motor_driver_pins(pipette_type, GPIOB); HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - + // Diag0 pin + GPIO_InitStruct.Pin = pipette_hardware_motor_driver_diag0_pin(pipette_type); + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); } } diff --git a/pipettes/firmware/stm32g4xx_it.c b/pipettes/firmware/stm32g4xx_it.c index 953d30404..de067fdd2 100644 --- a/pipettes/firmware/stm32g4xx_it.c +++ b/pipettes/firmware/stm32g4xx_it.c @@ -125,6 +125,8 @@ void EXTI2_IRQHandler(void) { void EXTI9_5_IRQHandler(void) { if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_7)) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_7); + } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_6)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6); } } @@ -132,6 +134,8 @@ void EXTI9_5_IRQHandler(void) { void EXTI15_10_IRQHandler(void) { if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_12)) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12); + } else if (__HAL_GPIO_EXTI_GET_IT(GPIO_PIN_11)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_11); } } diff --git a/pipettes/firmware/utility_gpio.c b/pipettes/firmware/utility_gpio.c index e5c76a8d6..3d0f40bb0 100644 --- a/pipettes/firmware/utility_gpio.c +++ b/pipettes/firmware/utility_gpio.c @@ -59,6 +59,16 @@ static void nvic_priority_enable_init() { /* EXTI interrupt init block tip sense*/ HAL_NVIC_SetPriority(block_2, 10, 0); HAL_NVIC_EnableIRQ(block_2); + + IRQn_Type block_15_10 = get_interrupt_line(gpio_block_15_10); + /* EXTI interrupt init block diag0*/ + HAL_NVIC_SetPriority(block_15_10, 5, 0); + HAL_NVIC_EnableIRQ(block_15_10); + } else { + IRQn_Type block_9_5 = get_interrupt_line(gpio_block_9_5); + /* EXTI interrupt init block diag0*/ + HAL_NVIC_SetPriority(block_9_5, 5, 0); + HAL_NVIC_EnableIRQ(block_9_5); } } diff --git a/pipettes/simulator/interfaces.cpp b/pipettes/simulator/interfaces.cpp index 8d0301c85..22251dc60 100644 --- a/pipettes/simulator/interfaces.cpp +++ b/pipettes/simulator/interfaces.cpp @@ -47,32 +47,43 @@ auto interfaces::get_interrupt_queues<PipetteType::THREE_EIGHTY_FOUR_CHANNEL>() auto linear_motor::get_interrupt( sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> { + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient> { return motor_handler::MotorInterruptHandler( - queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall, + queues.plunger_queue, linear_motor_tasks::get_queues(), + linear_motor_tasks::tmc2130_driver::get_queues(), hw, stall, queues.plunger_update_queue); } auto linear_motor::get_interrupt( sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, stall_check::StallCheck& stall) - -> MotorInterruptHandlerType<linear_motor_tasks::QueueClient> { + -> MotorInterruptHandlerType< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient> { return motor_handler::MotorInterruptHandler( - queues.plunger_queue, linear_motor_tasks::get_queues(), hw, stall, + queues.plunger_queue, linear_motor_tasks::get_queues(), + linear_motor_tasks::tmc2160_driver::get_queues(), hw, stall, queues.plunger_update_queue); } auto linear_motor::get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, LowThroughputInterruptQueues& queues, - MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler) + MotorInterruptHandlerType<linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient>& + handler) -> #ifdef USE_SENSOR_MOVE - -> motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient, + motor_messages::SensorSyncMove, sim_motor_hardware_iface::SimMotorHardwareIface> { #else - -> motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::Move, + motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2130_driver::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface> { #endif return motor_interrupt_driver::MotorInterruptDriver( @@ -82,14 +93,19 @@ auto linear_motor::get_interrupt_driver( auto linear_motor::get_interrupt_driver( sim_motor_hardware_iface::SimMotorHardwareIface& hw, HighThroughputInterruptQueues& queues, - MotorInterruptHandlerType<linear_motor_tasks::QueueClient>& handler) + MotorInterruptHandlerType<linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient>& + handler) -> #ifdef USE_SENSOR_MOVE - -> motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::SensorSyncMove, + motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient, + motor_messages::SensorSyncMove, sim_motor_hardware_iface::SimMotorHardwareIface> { #else - -> motor_interrupt_driver::MotorInterruptDriver< - linear_motor_tasks::QueueClient, motor_messages::Move, + motor_interrupt_driver::MotorInterruptDriver< + linear_motor_tasks::QueueClient, + linear_motor_tasks::tmc2160_driver::QueueClient, motor_messages::Move, sim_motor_hardware_iface::SimMotorHardwareIface> { #endif return motor_interrupt_driver::MotorInterruptDriver( @@ -136,10 +152,12 @@ auto gear_motor::get_interrupts(gear_motor::GearHardware& hw, return gear_motor::GearInterruptHandlers{ .left = motor_handler::MotorInterruptHandler( queues.left_motor_queue, gear_motor_tasks::get_left_gear_queues(), - hw.left, stall.left, queues.left_update_queue), + gear_motor_tasks::get_left_gear_queues(), hw.left, stall.left, + queues.left_update_queue), .right = motor_handler::MotorInterruptHandler( queues.right_motor_queue, gear_motor_tasks::get_right_gear_queues(), - hw.right, stall.right, queues.right_update_queue), + gear_motor_tasks::get_right_gear_queues(), hw.right, stall.right, + queues.right_update_queue), }; } diff --git a/sensors/tests/CMakeLists.txt b/sensors/tests/CMakeLists.txt index 55f9d034b..982b40967 100644 --- a/sensors/tests/CMakeLists.txt +++ b/sensors/tests/CMakeLists.txt @@ -36,7 +36,7 @@ target_compile_options(sensors $<$<COMPILE_LANGUAGE:CXX>:-fno-rtti> ) -target_link_libraries(sensors PUBLIC Catch2::Catch2 common-core motor-utils common-simulation) +target_link_libraries(sensors PUBLIC Catch2::Catch2 Boost::boost Boost::program_options Boost::date_time common-core motor-utils common-simulation) target_i2c_simlib(sensors)