Skip to content

Commit

Permalink
implemented motor driver error surfacing for head board
Browse files Browse the repository at this point in the history
  • Loading branch information
@pmoegenburg
pmoegenburg committed Jan 3, 2024
1 parent 19ace41 commit cd19068
Show file tree
Hide file tree
Showing 9 changed files with 149 additions and 23 deletions.
19 changes: 17 additions & 2 deletions head/core/tasks_proto.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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);
Expand Down Expand Up @@ -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
Expand Down
19 changes: 17 additions & 2 deletions head/core/tasks_rev1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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);
Expand Down Expand Up @@ -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
Expand Down
33 changes: 26 additions & 7 deletions head/firmware/main_proto.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include <array>
#include <cstdio>
#include <cstring>
#include <tuple>

// clang-format off
#include "FreeRTOS.h"
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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},
Expand All @@ -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},
Expand Down Expand Up @@ -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();
Expand All @@ -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,
Expand Down
28 changes: 24 additions & 4 deletions head/firmware/main_rev1.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include <array>
#include <cstdio>
#include <cstring>
#include <tuple>

// clang-format off
#include "FreeRTOS.h"
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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},
Expand All @@ -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},
Expand Down Expand Up @@ -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);
Expand All @@ -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,
Expand Down
5 changes: 4 additions & 1 deletion head/firmware/motor_hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
43 changes: 42 additions & 1 deletion head/firmware/motor_hardware_common.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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'
Expand Down Expand Up @@ -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);
Expand Down
11 changes: 11 additions & 0 deletions head/firmware/stm32g4xx_it.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
7 changes: 4 additions & 3 deletions include/head/core/tasks_proto.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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.
Expand Down
Loading

0 comments on commit cd19068

Please sign in to comment.