diff --git a/head/core/tasks_proto.cpp b/head/core/tasks_proto.cpp index 51ec208e5..9567d6011 100644 --- a/head/core/tasks_proto.cpp +++ b/head/core/tasks_proto.cpp @@ -78,10 +78,23 @@ 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 +108,7 @@ 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); @@ -201,6 +214,8 @@ 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 diff --git a/head/core/tasks_rev1.cpp b/head/core/tasks_rev1.cpp index dedb0aca3..3b618f4e7 100644 --- a/head/core/tasks_rev1.cpp +++ b/head/core/tasks_rev1.cpp @@ -83,10 +83,23 @@ 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 +113,7 @@ 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); @@ -222,6 +235,8 @@ 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 diff --git a/head/firmware/main_proto.cpp b/head/firmware/main_proto.cpp index c63842b32..5d4a44cfa 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) @@ -146,7 +150,13 @@ struct motor_hardware::HardwareConfig pin_configurations_left { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOB, .pin = GPIO_PIN_4, - .active_setting = GPIO_PIN_RESET} + .active_setting = GPIO_PIN_RESET}, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_13, + .active_setting = GPIO_PIN_RESET} }; struct motor_hardware::HardwareConfig pin_configurations_right { @@ -190,14 +200,22 @@ struct motor_hardware::HardwareConfig pin_configurations_right { // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) .port = GPIOB, .pin = GPIO_PIN_4, - .active_setting = GPIO_PIN_RESET} + .active_setting = GPIO_PIN_RESET}, + .diag0 = + { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-cstyle-cast) + .port = GPIOC, + .pin = GPIO_PIN_15, + .active_setting = GPIO_PIN_RESET} }; // TODO clean up the head main file by using interfaces. static tmc2130::configs::TMC2130DriverConfig motor_driver_configs_right{ .registers = { - .gconfig = {.en_pwm_mode = 1}, + .gconfig = {.en_pwm_mode = 1, + .diag0_error = 1, + .diag0_otpw = 1}, .ihold_irun = {.hold_current = 0xB, .run_current = 0x19, .hold_current_delay = 0x7}, @@ -224,7 +242,9 @@ 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, + .diag0_otpw = 1}, .ihold_irun = {.hold_current = 0xB, .run_current = 0x19, .hold_current_delay = 0x7}, @@ -395,7 +415,7 @@ 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(); @@ -409,8 +429,7 @@ 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, + 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, diff --git a/head/firmware/main_rev1.cpp b/head/firmware/main_rev1.cpp index 008560e77..dfa4317e6 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,9 @@ 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, + .diag0_otpw = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -232,7 +250,9 @@ 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, + .diag0_otpw = 1}, .ihold_irun = {.hold_current = 16, .run_current = 31, .hold_current_delay = 0x7}, @@ -420,7 +440,7 @@ 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); @@ -434,7 +454,7 @@ auto main() -> int { utility_gpio_init(); can_bus_1.start(can_bit_timings); - head_tasks::start_tasks(can_bus_1, motor_left.motion_controller, + 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, diff --git a/head/firmware/motor_hardware.h b/head/firmware/motor_hardware.h index 85981ae3d..b3a760ee0 100644 --- a/head/firmware/motor_hardware.h +++ b/head/firmware/motor_hardware.h @@ -14,10 +14,13 @@ 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 00b141da5..8c9a22ff2 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' @@ -334,12 +355,32 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) { } } +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + 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 57ba8aef7..6cc5709bc 100644 --- a/head/firmware/stm32g4xx_it.c +++ b/head/firmware/stm32g4xx_it.c @@ -168,6 +168,17 @@ void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); } */ void TIM3_IRQHandler(void) { HAL_TIM_IRQHandler(&htim3); } +void EXTI15_10_IRQHandler(void) { + // make this get/deduce pin! Needs to work for C13/z and C15/a! + EXTI_HandleTypeDef hexti_13 = {.Line = EXTI_LINE_13}; + EXTI_HandleTypeDef hexti_15 = {.Line = EXTI_LINE_15}; + if (HAL_EXTI_GetPending(&hexti_13, 0)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13); + } else if (HAL_EXTI_GetPending(&hexti_15, 0)) { + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); + } +} + extern void xPortSysTickHandler(void); void SysTick_Handler(void) { HAL_IncTick(); diff --git a/include/head/core/tasks_proto.hpp b/include/head/core/tasks_proto.hpp index 6f87e5286..decacd3ab 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,7 @@ 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..9f6d0f84e 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,7 @@ 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.