From: LeonardoBizzoni Date: Tue, 17 Mar 2026 13:10:36 +0000 (+0100) Subject: Emergency mode + SW/HW emergency toggle X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;ds=sidebyside;p=pioneer-stm32 Emergency mode + SW/HW emergency toggle --- diff --git a/pioneer_controller/.settings/language.settings.xml b/pioneer_controller/.settings/language.settings.xml index a1c742c..6d20c96 100644 --- a/pioneer_controller/.settings/language.settings.xml +++ b/pioneer_controller/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -17,7 +17,7 @@ - + diff --git a/pioneer_controller/Core/Inc/firmware/fmw_core.h b/pioneer_controller/Core/Inc/firmware/fmw_core.h index df23782..00c4fcd 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -3,10 +3,11 @@ typedef uint8_t FMW_Mode; enum { - FMW_Mode_None = 0, - FMW_Mode_Config = 1, - FMW_Mode_Run = 2, - FMW_Mode_COUNT = 3, + FMW_Mode_None = 0, + FMW_Mode_Config = 1, + FMW_Mode_Run = 2, + FMW_Mode_Emergency = 3, + FMW_Mode_COUNT = 4, }; typedef struct FMW_Encoder { @@ -79,6 +80,39 @@ typedef struct FMW_Hook { void *args; } FMW_Hook; +typedef struct FMW_InitInfo { + struct { + UART_HandleTypeDef *huart; + CRC_HandleTypeDef *hcrc; + void (*handler)(FMW_Message *msg, CRC_HandleTypeDef *hcrc); + } message_exchange; + + struct { + TIM_HandleTypeDef *timer; + void (*on_begin)(void); + void (*on_end)(void); + uint32_t wait_at_most_ms_before_emergency; + } emergency; + + FMW_Motor *motors; + int32_t motors_count; +} FMW_InitInfo; + +void fmw_init(const FMW_InitInfo *info) __attribute__((nonnull)); + +void fmw_uart_message_dispatch(void); +void fmw_uart_message_send(FMW_Message *msg) __attribute__((nonnull)); +void fmw_uart_error(void); + +void fmw_emergency_begin(void); +void fmw_emergency_end(void); +void fmw_emergency_timer_update(void); + +FMW_Mode fmw_mode_current(void); +FMW_Mode fmw_mode_transition(FMW_Mode mode); + +FMW_Result fmw_result_from_uart_error(void) __attribute__((nonnull, warn_unused_result)); + void fmw_motors_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); void fmw_motors_deinit(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); void fmw_motors_stop(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); @@ -103,15 +137,11 @@ void fmw_led_update(FMW_Led *led) __attribute__((nonnull)); void fmw_buzzers_set(FMW_Buzzer buzzer[], int32_t count, bool on) __attribute__((nonnull)); -FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) __attribute__((warn_unused_result, nonnull)); -HAL_StatusTypeDef fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, - FMW_Message *msg, int32_t wait_ms) __attribute__((nonnull, warn_unused_result)); -FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) __attribute__((nonnull, warn_unused_result)); - Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) __attribute__((nonnull)); #define FMW_LED_UPDATE_PERIOD 200 #define FMW_DEBOUNCE_DELAY 200 +#define FMW_EMERGENCY_MODE_EXIT_BUTTON_HOLD_DURATION_MS 2000 #define FMW_V_REF 3.3f #define FMW_ADC_RESOLUTION 4095.0f diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index b2c08f0..e17ea98 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -25,7 +25,9 @@ typedef union { X(FMW_MessageType_Config_LED, 6) \ X(FMW_MessageType_Run_GetStatus, 7) \ X(FMW_MessageType_Run_SetVelocity, 8) \ - X(FMW_MessageType_COUNT, 9) + X(FMW_MessageType_Emergency_Begin, 9) \ + X(FMW_MessageType_Emergency_End, 10) \ + X(FMW_MessageType_COUNT, 11) // NOTE(lb): Here i want to take all of the variants that are generated from the macro // expension of FMW_MESSAGE_TYPE_VARIANTS and use them to populate the enum definition. diff --git a/pioneer_controller/Core/Inc/stm32f7xx_it.h b/pioneer_controller/Core/Inc/stm32f7xx_it.h index 0b4ba8b..d589d76 100644 --- a/pioneer_controller/Core/Inc/stm32f7xx_it.h +++ b/pioneer_controller/Core/Inc/stm32f7xx_it.h @@ -62,6 +62,8 @@ void DMA1_Stream1_IRQHandler(void); void ADC_IRQHandler(void); void USART3_IRQHandler(void); void EXTI15_10_IRQHandler(void); +void TIM6_DAC_IRQHandler(void); +void TIM7_IRQHandler(void); /* USER CODE BEGIN EFP */ /* USER CODE END EFP */ diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index 7f94b83..0720bee 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -5,9 +5,30 @@ #include static struct { + void (*callback_message_handler)(FMW_Message *msg, CRC_HandleTypeDef *hcrc); + void (*callback_emergency_begin)(void); + void (*callback_emergency_end)(void); + + TIM_HandleTypeDef *emergency_timer; + uint32_t emergency_mode_grace_period_ms; + volatile uint32_t emergency_last_message_received_time; + FMW_Motor *motors; int32_t motors_count; -} fmw_state; + bool motors_active; + bool motors_active_before_emergency; + + UART_HandleTypeDef *huart; + CRC_HandleTypeDef *hcrc; + + volatile FMW_Message uart_buffer; + volatile FMW_Mode mode_current; + volatile FMW_Mode mode_previous; +} fmw_state = { + .emergency_mode_grace_period_ms = 1000, + .mode_current = FMW_Mode_Config, + .mode_previous = FMW_Mode_None, +}; static void fmw_hook_assert_fail(void *_) { if (fmw_state.motors != NULL) { @@ -15,6 +36,103 @@ static void fmw_hook_assert_fail(void *_) { } } +// ============================================================ +// Firmware initialization +void fmw_init(const FMW_InitInfo *info) { + FMW_ASSERT(info->message_exchange.huart != NULL); + FMW_ASSERT(info->message_exchange.hcrc != NULL); + FMW_ASSERT(info->message_exchange.handler != NULL); + FMW_ASSERT(info->emergency.on_begin != NULL); + FMW_ASSERT(info->emergency.on_end != NULL); + FMW_ASSERT(info->motors_count >= 0); + if (info->motors_count > 0) { FMW_ASSERT(info->motors != NULL); } + + fmw_state.motors = info->motors; + fmw_state.motors_count = info->motors_count; + fmw_state.huart = info->message_exchange.huart; + fmw_state.hcrc = info->message_exchange.hcrc; + fmw_state.emergency_mode_grace_period_ms = info->emergency.wait_at_most_ms_before_emergency; + fmw_state.callback_message_handler = info->message_exchange.handler; + fmw_state.callback_emergency_begin = info->emergency.on_begin; + fmw_state.callback_emergency_end = info->emergency.on_end; + fmw_state.emergency_timer = info->emergency.timer; + + HAL_StatusTypeDef timer_init_res = HAL_TIM_Base_Start_IT(info->emergency.timer); + FMW_ASSERT(timer_init_res == HAL_OK); + + HAL_StatusTypeDef dma_init_res = HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer); + FMW_ASSERT(dma_init_res == HAL_OK); +} + +void fmw_uart_message_dispatch(void) { + fmw_state.emergency_last_message_received_time = HAL_GetTick(); + fmw_state.callback_message_handler((FMW_Message*)&fmw_state.uart_buffer, fmw_state.hcrc); + // NOTE(lb): listen for the next message "recursively". + HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer); +} + +void fmw_uart_error(void) { + // NOTE(lb): i don't know how to determine if the error that cause the jump here + // was during a receive or a send of a message over UART, so i'm just + // going to stop the motors and abort the receive just in case. + fmw_motors_stop(fmw_state.motors, fmw_state.motors_count); + HAL_UART_AbortReceive(fmw_state.huart); + + FMW_Message response = {0}; + response.response.result = fmw_result_from_uart_error(); + fmw_uart_message_send(&response); + HAL_UART_Receive_DMA(fmw_state.huart, (uint8_t*)&fmw_state.uart_buffer, sizeof fmw_state.uart_buffer); +} + +void fmw_emergency_begin(void) { + if (fmw_state.mode_current == FMW_Mode_Emergency) { return; } + if (fmw_state.motors_count > 0) { + fmw_state.motors_active_before_emergency = fmw_state.motors[0].active; + if (fmw_state.motors[0].active) { + fmw_motors_stop(fmw_state.motors, fmw_state.motors_count); + fmw_motors_disable(fmw_state.motors, fmw_state.motors_count); + } + } + + fmw_state.mode_previous = fmw_state.mode_current; + fmw_state.mode_current = FMW_Mode_Emergency; + + if (fmw_state.callback_emergency_begin) { fmw_state.callback_emergency_begin(); } +} + +void fmw_emergency_end(void) { + if (fmw_state.mode_previous == FMW_Mode_None) { return; } + fmw_state.mode_current = fmw_state.mode_previous; + fmw_state.mode_previous = FMW_Mode_None; + if (fmw_state.motors_count > 0 && fmw_state.motors_active_before_emergency) { + fmw_motors_enable(fmw_state.motors, fmw_state.motors_count); + } + if (fmw_state.callback_emergency_end) { fmw_state.callback_emergency_end(); } +} + +void fmw_emergency_timer_update(void) { + if (fmw_state.mode_current != FMW_Mode_Run) { return; } + uint32_t time_now = HAL_GetTick(); + if (time_now - fmw_state.emergency_last_message_received_time > fmw_state.emergency_mode_grace_period_ms) { + fmw_emergency_begin(); + } +} + +FMW_Mode fmw_mode_current(void) { + return fmw_state.mode_current; +} + +FMW_Mode fmw_mode_transition(FMW_Mode mode) { + FMW_ASSERT(mode > FMW_Mode_None); + FMW_ASSERT(mode < FMW_Mode_COUNT); + FMW_Mode old = fmw_state.mode_previous; + fmw_state.mode_previous = fmw_state.mode_current; + fmw_state.mode_current = mode; + return old; +} + +// ============================================================ +// Misc FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, int32_t wait_ms) { FMW_ASSERT(wait_ms >= 0, .callback = fmw_hook_assert_fail); memset(msg, 0, sizeof *msg); @@ -27,37 +145,35 @@ FMW_Result fmw_message_uart_receive(UART_HandleTypeDef *huart, FMW_Message *msg, return FMW_Result_Ok; } -HAL_StatusTypeDef fmw_message_uart_send(UART_HandleTypeDef *huart, CRC_HandleTypeDef *hcrc, FMW_Message *msg, int32_t wait_ms) { - msg->header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)msg, sizeof *msg); - HAL_StatusTypeDef res = HAL_UART_Transmit(huart, (uint8_t*)msg, sizeof *msg, wait_ms); - return res; +void fmw_uart_message_send(FMW_Message *msg) { + msg->header.crc = HAL_CRC_Calculate(fmw_state.hcrc, (uint32_t*)msg, sizeof *msg); + HAL_StatusTypeDef res = HAL_UART_Transmit(fmw_state.huart, (uint8_t*)msg, sizeof *msg, fmw_state.emergency_mode_grace_period_ms); + if (res != HAL_OK) { fmw_emergency_begin(); } } -FMW_Message fmw_message_from_uart_error(const UART_HandleTypeDef *huart) { - FMW_ASSERT(huart->ErrorCode != HAL_UART_ERROR_NONE, .callback = fmw_hook_assert_fail); - FMW_Message res = {0}; - res.header.type = FMW_MessageType_Response; - switch (huart->ErrorCode) { +FMW_Result fmw_result_from_uart_error(void) { + FMW_ASSERT(fmw_state.huart->ErrorCode != HAL_UART_ERROR_NONE, .callback = fmw_hook_assert_fail); + switch (fmw_state.huart->ErrorCode) { case HAL_UART_ERROR_PE: { - res.response.result = FMW_Result_Error_UART_Parity; + return FMW_Result_Error_UART_Parity; } break; case HAL_UART_ERROR_FE: { - res.response.result = FMW_Result_Error_UART_Frame; + return FMW_Result_Error_UART_Frame; } break; case HAL_UART_ERROR_NE: { - res.response.result = FMW_Result_Error_UART_Noise; + return FMW_Result_Error_UART_Noise; } break; case HAL_UART_ERROR_ORE: { - res.response.result = FMW_Result_Error_UART_Overrun; + return FMW_Result_Error_UART_Overrun; } break; case HAL_UART_ERROR_RTO: { - res.response.result = FMW_Result_Error_UART_ReceiveTimeoutElapsed; + return FMW_Result_Error_UART_ReceiveTimeoutElapsed; } break; default: { // NOTE(lb): unreachable FMW_ASSERT(false, .callback = fmw_hook_assert_fail); } break; } - return res; + return FMW_Result_Ok; } Vec2Float fmw_setpoint_from_velocities(const FMW_Odometry *odometry, float linear, float angular) { @@ -228,7 +344,7 @@ void fmw_encoders_update(FMW_Encoder encoders[], int32_t count) { float fmw_encoder_get_linear_velocity(const FMW_Encoder *encoder, float meters_traveled) { float deltatime = encoder->current_millis - encoder->previous_millis; - FMW_ASSERT(deltatime > 0.f, .callback = fmw_hook_assert_fail); + if (deltatime == 0.f) { return 0.f; } float linear_velocity = meters_traveled / (deltatime / 1000.f); return linear_velocity; } diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index 7fe6158..53fddf0 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -32,8 +32,10 @@ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ -FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) - __attribute__((nonnull)); +void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) __attribute__((nonnull)); + +void emergency_mode_begin(void); +void emergency_mode_end(void); /* USER CODE END PTD */ /* Private define ------------------------------------------------------------*/ @@ -55,6 +57,7 @@ TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim4; TIM_HandleTypeDef htim6; +TIM_HandleTypeDef htim7; UART_HandleTypeDef huart3; DMA_HandleTypeDef hdma_usart3_rx; @@ -160,9 +163,6 @@ static uint32_t led_update_period = 200; static volatile int32_t ticks_left = 0; static volatile int32_t ticks_right = 0; -static volatile FMW_Message uart_message_buffer = {0}; -static volatile uint32_t time_last_motors = 0; -static volatile FMW_Mode current_mode = FMW_Mode_Config; /* USER CODE END PV */ @@ -178,6 +178,7 @@ static void MX_TIM2_Init(void); static void MX_TIM3_Init(void); static void MX_TIM6_Init(void); static void MX_CRC_Init(void); +static void MX_TIM7_Init(void); /* USER CODE BEGIN PFP */ /* USER CODE END PFP */ @@ -223,6 +224,7 @@ int main(void) MX_TIM3_Init(); MX_TIM6_Init(); MX_CRC_Init(); + MX_TIM7_Init(); /* USER CODE BEGIN 2 */ start(); @@ -642,6 +644,44 @@ static void MX_TIM6_Init(void) } +/** + * @brief TIM7 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM7_Init(void) +{ + + /* USER CODE BEGIN TIM7_Init 0 */ + + /* USER CODE END TIM7_Init 0 */ + + TIM_MasterConfigTypeDef sMasterConfig = {0}; + + /* USER CODE BEGIN TIM7_Init 1 */ + + /* USER CODE END TIM7_Init 1 */ + htim7.Instance = TIM7; + htim7.Init.Prescaler = 10799; + htim7.Init.CounterMode = TIM_COUNTERMODE_UP; + htim7.Init.Period = 9999; + htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim7) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM7_Init 2 */ + + /* USER CODE END TIM7_Init 2 */ + +} + /** * @brief USART3 Initialization Function * @param None @@ -729,7 +769,7 @@ static void MX_GPIO_Init(void) /*Configure GPIO pin : USER_Btn_Pin */ GPIO_InitStruct.Pin = USER_Btn_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(USER_Btn_GPIO_Port, &GPIO_InitStruct); @@ -796,11 +836,25 @@ static void MX_GPIO_Init(void) /* USER CODE BEGIN 4 */ void start(void) { - // Enables UART RX interrupt - HAL_UART_Receive_DMA(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); + FMW_InitInfo fmw_info = { + .motors = motors.values, + .motors_count = ARRLENGTH(motors.values), + .emergency = { + .timer = &htim7, + .on_begin = emergency_mode_begin, + .on_end = emergency_mode_end, + .wait_at_most_ms_before_emergency = 2000, + }, + .message_exchange = { + .huart = UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, + .hcrc = &hcrc, + .handler = message_handler, + }, + }; + fmw_init(&fmw_info); for (;;) { - switch (current_mode) { + switch (fmw_mode_current()) { case FMW_Mode_Config: { } break; case FMW_Mode_Run: { @@ -829,19 +883,33 @@ Mat3Float mat3float_multiply(Mat3Float lhs, Mat3Float rhs) { return output; } -FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { +void emergency_mode_begin(void) { + HAL_GPIO_TogglePin(GPIOB, LD1_Pin | LD2_Pin | LD3_Pin); + fmw_encoder_count_reset(&encoders.left); + fmw_encoder_count_reset(&encoders.right); +} + +void emergency_mode_end(void) { + HAL_GPIO_TogglePin(GPIOB, LD1_Pin | LD2_Pin | LD3_Pin); +} + +void message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { // NOTE(lb): the `msg->header.crc != -1` checks are just because i haven't // implemented CRC into the program that sends these messages. // i also don't know if the code to calculate CRC is correct (probably isn't). + + FMW_Result result = FMW_Result_Ok; if (msg->header.crc != -1) { uint32_t crc_received = msg->header.crc; msg->header.crc = 0; uint32_t crc_computed = HAL_CRC_Calculate(hcrc, (uint32_t*)msg, sizeof *msg); - if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; } + if (crc_computed != crc_received) { + result = FMW_Result_Error_UART_Crc; + goto msg_contains_error; + } } - FMW_Result result = FMW_Result_Ok; - switch (current_mode) { + switch (fmw_mode_current()) { case FMW_Mode_Config: { switch (msg->header.type) { case FMW_MessageType_ModeChange_Run: { @@ -858,7 +926,7 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6); FMW_ASSERT(timer_status == HAL_OK); - current_mode = FMW_Mode_Run; + fmw_mode_transition(FMW_Mode_Run); } break; case FMW_MessageType_Config_Robot: { if (!(msg->config_robot.baseline > 0.f)) { @@ -898,6 +966,10 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { pled.voltage_hysteresis = msg->config_led.voltage_hysteresis; led_update_period = msg->config_led.update_period; } break; + case FMW_MessageType_Emergency_Begin: { + fmw_emergency_begin(); + } break; + case FMW_MessageType_Emergency_End: case FMW_MessageType_ModeChange_Config: case FMW_MessageType_Run_GetStatus: case FMW_MessageType_Run_SetVelocity: { @@ -943,9 +1015,8 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { msg.response.orientation_y = odometry.orientation.y; msg.response.velocity_linear = (odometry.velocity_linear.left + odometry.velocity_linear.right) / 2.f; msg.response.velocity_angular = odometry.velocity_angular; - msg.header.crc = HAL_CRC_Calculate(hcrc, (uint32_t*)&msg, sizeof msg); - (void)HAL_UART_Transmit(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, (uint8_t*)&msg, sizeof msg, HAL_MAX_DELAY); - return FMW_Result_Ok; + fmw_uart_message_send(&msg); + return; // NOTE(lb): GetStatus&SetVelocity have to respond with the same special message format. // so they don't continue down to `msg_contains_error`. } break; @@ -961,8 +1032,12 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { HAL_StatusTypeDef timer_status = HAL_TIM_Base_Stop_IT(&htim6); FMW_ASSERT(timer_status == HAL_OK); - current_mode = FMW_Mode_Config; + fmw_mode_transition(FMW_Mode_Config); + } break; + case FMW_MessageType_Emergency_Begin: { + fmw_emergency_begin(); } break; + case FMW_MessageType_Emergency_End: case FMW_MessageType_ModeChange_Run: case FMW_MessageType_Config_Robot: case FMW_MessageType_Config_PID: @@ -976,24 +1051,31 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { } break; } } break; + case FMW_Mode_Emergency: { + switch (msg->header.type) { + case FMW_MessageType_Emergency_End: { + fmw_emergency_end(); + } break; + default: { + result = FMW_Result_Error_Command_NotAvailable; + } break; + } + } break; } // NOTE(lb): control flow naturally converges here. // the symbol is used to jump here directly in case of error. msg_contains_error:; - FMW_Message response = {0}; - response.header.type = FMW_MessageType_Response; - response.response.result = result; - HAL_StatusTypeDef send_res = fmw_message_uart_send(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, hcrc, &response, 1); - FMW_ASSERT(send_res == HAL_OK); - return result; + FMW_Message msg_response = {0}; + msg_response.header.type = FMW_MessageType_Response; + msg_response.response.result = result; + fmw_uart_message_send(&msg_response); } -// TIMER 100Hz PID control void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { - if (htim != &htim6) { return; } - - { // PID control + if (htim == &htim7) { // TIMER 1Hz no message exchange check + fmw_emergency_timer_update(); + } else if (htim == &htim6) { // TIMER 100Hz PID control fmw_encoders_update(encoders.values, ARRLENGTH(encoders.values)); ticks_left += encoders.left.ticks; ticks_right += encoders.right.ticks; @@ -1022,42 +1104,42 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { } void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - if (huart != UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL) { return; } - (void)message_handler((FMW_Message*)&uart_message_buffer, &hcrc); - - // NOTE(lb): listen for the next message "recursively". - HAL_UART_Receive_DMA(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); + if (huart == UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL) { + fmw_uart_message_dispatch(); + return; + } } void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { - if (huart != UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL) { return; } - - // NOTE(lb): i don't know how to determine if the error that cause the jump here - // was during a receive or a send of a message over UART, so i'm just - // going to stop the motors and abort the receive just in case. - fmw_motors_stop(motors.values, ARRLENGTH(motors.values)); - HAL_UART_AbortReceive(huart); - - FMW_Message response = fmw_message_from_uart_error(huart); - retry:; - HAL_StatusTypeDef res = fmw_message_uart_send(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, &hcrc, &response, HAL_MAX_DELAY); - if (res != HAL_OK) { - // NOTE(lb): keep trying to send the error message even on failure until it succeeds - // while also being extra annoying. - fmw_buzzers_set(&buzzer, 1, true); - goto retry; + if (huart == UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL) { + fmw_uart_error(); + return; } - fmw_buzzers_set(&buzzer, 1, false); - - // NOTE(lb): go back to normal message receive after the error has been notified - HAL_UART_Receive_DMA(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); } void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { uint32_t time_now = HAL_GetTick(); switch (GPIO_Pin) { + case USER_Btn_Pin: { + static uint32_t time_btn_press_start = 0; + static uint32_t time_last_emergency = 0; + if (time_now - time_last_emergency <= FMW_DEBOUNCE_DELAY) { return; } + time_last_emergency = time_now; + GPIO_PinState btn_state = HAL_GPIO_ReadPin(USER_Btn_GPIO_Port, USER_Btn_Pin); + if (btn_state == GPIO_PIN_SET) { + time_btn_press_start = time_now; + } else { + uint32_t btn_held_duration = time_now - time_btn_press_start; + if (btn_held_duration >= FMW_EMERGENCY_MODE_EXIT_BUTTON_HOLD_DURATION_MS) { + fmw_emergency_end(); + } else { + fmw_emergency_begin(); + } + } + } break; case motors_btn_Pin: { + static uint32_t time_last_motors = 0; if (time_now - time_last_motors > FMW_DEBOUNCE_DELAY) { time_last_motors = time_now; if (motors.left.active && motors.right.active) { @@ -1080,12 +1162,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { response.header.type = FMW_MessageType_Response; response.response.result = FMW_Result_Error_FaultPinTriggered; - retry:; - HAL_StatusTypeDef res = fmw_message_uart_send(UART_HANDLE_PTR_USED_FOR_MESSAGE_EXCHANGE_PROTOCOL, &hcrc, &response, HAL_MAX_DELAY); - if (res != HAL_OK) { // NOTE(lb): send help - fmw_buzzers_set(&buzzer, 1, true); - goto retry; - } + fmw_uart_message_send(&response); HAL_TIM_Base_Stop_IT(&htim6); } break; diff --git a/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c b/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c index 44e3475..71bbe65 100644 --- a/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c +++ b/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c @@ -301,10 +301,26 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base) /* USER CODE END TIM6_MspInit 0 */ /* Peripheral clock enable */ __HAL_RCC_TIM6_CLK_ENABLE(); + /* TIM6 interrupt Init */ + HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn); /* USER CODE BEGIN TIM6_MspInit 1 */ /* USER CODE END TIM6_MspInit 1 */ + } + else if(htim_base->Instance==TIM7) + { + /* USER CODE BEGIN TIM7_MspInit 0 */ + + /* USER CODE END TIM7_MspInit 0 */ + /* Peripheral clock enable */ + __HAL_RCC_TIM7_CLK_ENABLE(); + /* TIM7 interrupt Init */ + HAL_NVIC_SetPriority(TIM7_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(TIM7_IRQn); + /* USER CODE BEGIN TIM7_MspInit 1 */ + /* USER CODE END TIM7_MspInit 1 */ } } @@ -452,10 +468,27 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base) /* USER CODE END TIM6_MspDeInit 0 */ /* Peripheral clock disable */ __HAL_RCC_TIM6_CLK_DISABLE(); + + /* TIM6 interrupt DeInit */ + HAL_NVIC_DisableIRQ(TIM6_DAC_IRQn); /* USER CODE BEGIN TIM6_MspDeInit 1 */ /* USER CODE END TIM6_MspDeInit 1 */ } + else if(htim_base->Instance==TIM7) + { + /* USER CODE BEGIN TIM7_MspDeInit 0 */ + + /* USER CODE END TIM7_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM7_CLK_DISABLE(); + + /* TIM7 interrupt DeInit */ + HAL_NVIC_DisableIRQ(TIM7_IRQn); + /* USER CODE BEGIN TIM7_MspDeInit 1 */ + + /* USER CODE END TIM7_MspDeInit 1 */ + } } diff --git a/pioneer_controller/Core/Src/stm32f7xx_it.c b/pioneer_controller/Core/Src/stm32f7xx_it.c index 117a7c4..339aa6d 100644 --- a/pioneer_controller/Core/Src/stm32f7xx_it.c +++ b/pioneer_controller/Core/Src/stm32f7xx_it.c @@ -57,6 +57,8 @@ /* External variables --------------------------------------------------------*/ extern ADC_HandleTypeDef hadc1; +extern TIM_HandleTypeDef htim6; +extern TIM_HandleTypeDef htim7; extern DMA_HandleTypeDef hdma_usart3_rx; extern UART_HandleTypeDef huart3; /* USER CODE BEGIN EV */ @@ -284,6 +286,34 @@ void EXTI15_10_IRQHandler(void) /* USER CODE END EXTI15_10_IRQn 1 */ } +/** + * @brief This function handles TIM6 global interrupt, DAC1 and DAC2 underrun error interrupts. + */ +void TIM6_DAC_IRQHandler(void) +{ + /* USER CODE BEGIN TIM6_DAC_IRQn 0 */ + + /* USER CODE END TIM6_DAC_IRQn 0 */ + HAL_TIM_IRQHandler(&htim6); + /* USER CODE BEGIN TIM6_DAC_IRQn 1 */ + + /* USER CODE END TIM6_DAC_IRQn 1 */ +} + +/** + * @brief This function handles TIM7 global interrupt. + */ +void TIM7_IRQHandler(void) +{ + /* USER CODE BEGIN TIM7_IRQn 0 */ + + /* USER CODE END TIM7_IRQn 0 */ + HAL_TIM_IRQHandler(&htim7); + /* USER CODE BEGIN TIM7_IRQn 1 */ + + /* USER CODE END TIM7_IRQn 1 */ +} + /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ diff --git a/pioneer_controller/pioneer_controller.ioc b/pioneer_controller/pioneer_controller.ioc index ab3cea3..f23ff07 100644 --- a/pioneer_controller/pioneer_controller.ioc +++ b/pioneer_controller/pioneer_controller.ioc @@ -29,7 +29,8 @@ Mcu.IP0=ADC1 Mcu.IP1=CORTEX_M7 Mcu.IP10=TIM4 Mcu.IP11=TIM6 -Mcu.IP12=USART3 +Mcu.IP12=TIM7 +Mcu.IP13=USART3 Mcu.IP2=CRC Mcu.IP3=DMA Mcu.IP4=NVIC @@ -38,7 +39,7 @@ Mcu.IP6=SYS Mcu.IP7=TIM1 Mcu.IP8=TIM2 Mcu.IP9=TIM3 -Mcu.IPNb=13 +Mcu.IPNb=14 Mcu.Name=STM32F767ZITx Mcu.Package=LQFP144 Mcu.Pin0=PC13 @@ -67,13 +68,14 @@ Mcu.Pin29=VP_CRC_VS_CRC Mcu.Pin3=PH0/OSC_IN Mcu.Pin30=VP_SYS_VS_Systick Mcu.Pin31=VP_TIM6_VS_ClockSourceINT +Mcu.Pin32=VP_TIM7_VS_ClockSourceINT Mcu.Pin4=PH1/OSC_OUT Mcu.Pin5=PC0 Mcu.Pin6=PA0/WKUP Mcu.Pin7=PA1 Mcu.Pin8=PA2 Mcu.Pin9=PA3 -Mcu.PinsNb=32 +Mcu.PinsNb=33 Mcu.ThirdPartyNb=0 Mcu.UserConstants= Mcu.UserName=STM32F767ZITx @@ -94,6 +96,8 @@ NVIC.PendSV_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false NVIC.SysTick_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:false +NVIC.TIM6_DAC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true +NVIC.TIM7_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true NVIC.USART3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false PA0/WKUP.GPIOParameters=GPIO_Label @@ -144,7 +148,7 @@ PC0.Locked=true PC0.Signal=GPIO_Output PC13.GPIOParameters=GPIO_Label,GPIO_ModeDefaultEXTI PC13.GPIO_Label=USER_Btn [B1] -PC13.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING +PC13.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_RISING_FALLING PC13.Locked=true PC13.Signal=GPXTI13 PC14/OSC32_IN.Locked=true @@ -243,7 +247,7 @@ ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= ProjectManager.UnderRoot=true -ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,4-MX_USART3_UART_Init-USART3-false-HAL-true,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM4_Init-TIM4-false-HAL-true,7-MX_TIM1_Init-TIM1-false-HAL-true,8-MX_TIM2_Init-TIM2-false-HAL-true,9-MX_TIM3_Init-TIM3-false-HAL-true,10-MX_TIM6_Init-TIM6-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,0-MX_DMA_Init-DMA-false-HAL-true +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_USART3_UART_Init-USART3-false-HAL-true,5-MX_ADC1_Init-ADC1-false-HAL-true,6-MX_TIM4_Init-TIM4-false-HAL-true,7-MX_TIM1_Init-TIM1-false-HAL-true,8-MX_TIM2_Init-TIM2-false-HAL-true,9-MX_TIM3_Init-TIM3-false-HAL-true,10-MX_TIM6_Init-TIM6-false-HAL-true,11-MX_CRC_Init-CRC-false-HAL-true,12-MX_TIM7_Init-TIM7-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-true RCC.48MHZClocksFreq_Value=24000000 RCC.ADC12outputFreq_Value=72000000 RCC.ADC34outputFreq_Value=72000000 @@ -369,6 +373,10 @@ TIM4.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 TIM4.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4 TIM4.IPParameters=Channel-PWM Generation3 CH3,Channel-PWM Generation4 CH4,Period TIM4.Period=3840 +TIM7.AutoReloadPreload=TIM_AUTORELOAD_PRELOAD_DISABLE +TIM7.IPParameters=Prescaler,AutoReloadPreload,Period +TIM7.Period=9999 +TIM7.Prescaler=10799 USART3.IPParameters=VirtualMode-Asynchronous USART3.VirtualMode-Asynchronous=VM_ASYNC VP_CRC_VS_CRC.Mode=CRC_Activate @@ -377,6 +385,8 @@ VP_SYS_VS_Systick.Mode=SysTick VP_SYS_VS_Systick.Signal=SYS_VS_Systick VP_TIM6_VS_ClockSourceINT.Mode=Enable_Timer VP_TIM6_VS_ClockSourceINT.Signal=TIM6_VS_ClockSourceINT +VP_TIM7_VS_ClockSourceINT.Mode=Enable_Timer +VP_TIM7_VS_ClockSourceINT.Signal=TIM7_VS_ClockSourceINT board=NUCLEO-F767ZI boardIOC=true isbadioc=false diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/CMakeLists.txt b/pioneer_workstation_ws/src/pioneer3dx_controller/CMakeLists.txt index cd1290b..7269b6d 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/CMakeLists.txt +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/CMakeLists.txt @@ -17,10 +17,12 @@ rosidl_generate_interfaces(${PROJECT_NAME} "messages/ConfigLed.msg" "messages/ConfigPid.msg" "messages/ConfigRobot.msg" + "messages/EmergencyMode.msg" "services/ChangeMode.srv" "services/ConfigLed.srv" "services/ConfigPid.srv" "services/ConfigRobot.srv" + "services/EmergencyMode.srv" DEPENDENCIES service_msgs ) diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/messages/ChangeMode.msg b/pioneer_workstation_ws/src/pioneer3dx_controller/messages/ChangeMode.msg index 56f716d..9232099 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/messages/ChangeMode.msg +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/messages/ChangeMode.msg @@ -1 +1 @@ -byte mode \ No newline at end of file +uint8 mode \ No newline at end of file diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp index a1a6a68..78b190e 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.cpp @@ -13,7 +13,6 @@ using namespace std::chrono_literals; P3DX_Controller_Node::P3DX_Controller_Node(int32_t serial_fd) : rclcpp::Node("P3DX_Controller_Node"), serial_fd(serial_fd), - stm32_config_mode(false), publisher_odometry(this->create_publisher("odometry", 10)), subscriber_velocity(this->create_subscription("p3dx_command_velocity", 10, std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity, @@ -26,6 +25,8 @@ P3DX_Controller_Node::P3DX_Controller_Node(int32_t serial_fd) : &P3DX_Controller_Node::callback_service_command_config_robot_s)), service_config_led(this->create_service("p3dx_command_config_led", &P3DX_Controller_Node::callback_service_command_config_led_s)), + service_emergency_mode(this->create_service("p3dx_command_emergency_mode", + &P3DX_Controller_Node::callback_service_command_emergency_mode_s)), timer_publisher_odometry(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this))) { assert(this->serial_fd != -1); @@ -35,20 +36,15 @@ P3DX_Controller_Node::P3DX_Controller_Node(int32_t serial_fd) : void P3DX_Controller_Node::callback_publish_odometry(void) { - if (this->stm32_config_mode) { return; } - FMW_Message msg_get_status; msg_get_status.header.type = FMW_MessageType_Run_GetStatus; msg_get_status.header.crc = (uint32_t)-1; FMW_Message response = this->stm32_message_send(&msg_get_status); assert(response.header.type == FMW_MessageType_Response); - this->stm32_message_print(&response); if (response.response.result == FMW_Result_Error_Command_NotAvailable) { - this->stm32_config_mode = true; return; - } else { - this->stm32_config_mode = false; } + this->stm32_message_print(&response); nav_msgs::msg::Odometry output; output.header.frame_id = this->get_parameter("frame_id_odometry").as_string(); @@ -67,8 +63,6 @@ void P3DX_Controller_Node::callback_publish_odometry(void) void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd) { - if (this->stm32_config_mode) { return; } - FMW_Message msg; ::memset(&msg, 0, sizeof(msg)); msg.header.type = FMW_MessageType_Run_SetVelocity; @@ -77,6 +71,7 @@ void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_ms msg.run_set_velocity.angular = cmd->angular.x; FMW_Message response_msg = this->stm32_message_send(&msg); + this->stm32_message_print(&response_msg); assert(response_msg.header.type == FMW_MessageType_Response); assert(response_msg.response.result == FMW_Result_Ok); } @@ -111,11 +106,9 @@ void P3DX_Controller_Node::callback_service_command_change_mode_s(const std::sha msg.header.crc = (uint32_t)-1; FMW_Message response_msg = p3dx_controller->stm32_message_send(&msg); + p3dx_controller->stm32_message_print(&response_msg); assert(response_msg.header.type == FMW_MessageType_Response); response->status_response = (uint8_t)response_msg.response.result; - if (response_msg.response.result == FMW_Result_Ok) { - p3dx_controller->stm32_config_mode = (mode == P3DX_Cmd_ChangeMode_Kind::Config); - } } } @@ -138,6 +131,7 @@ void P3DX_Controller_Node::callback_service_command_config_pid_s(const std::shar ::memcpy(msg.config_pid.cross.values, request->cross.data(), sizeof(msg.config_pid.left.values)); FMW_Message response_msg = p3dx_controller->stm32_message_send(&msg); + p3dx_controller->stm32_message_print(&response_msg); assert(response_msg.header.type == FMW_MessageType_Response); response->status_response = (uint8_t)response_msg.response.result; } @@ -164,6 +158,7 @@ void P3DX_Controller_Node::callback_service_command_config_robot_s(const std::sh msg.config_robot.ticks_per_revolution_right = request->ticks_per_revolution_right; FMW_Message response_msg = p3dx_controller->stm32_message_send(&msg); + p3dx_controller->stm32_message_print(&response_msg); assert(response_msg.header.type == FMW_MessageType_Response); response->status_response = (uint8_t)response_msg.response.result; } @@ -187,6 +182,23 @@ void P3DX_Controller_Node::callback_service_command_config_led_s(const std::shar msg.config_led.update_period = request->update_period; FMW_Message response_msg = p3dx_controller->stm32_message_send(&msg); + p3dx_controller->stm32_message_print(&response_msg); + assert(response_msg.header.type == FMW_MessageType_Response); + response->status_response = (uint8_t)response_msg.response.result; +} + +void P3DX_Controller_Node::callback_service_command_emergency_mode_s(const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO(p3dx_controller->get_logger(), "emergency-mode %s", (request->enable ? "entered" : "exit")); + + FMW_Message msg; + ::memset(&msg, 0, sizeof(msg)); + msg.header.type = (request->enable ? FMW_MessageType_Emergency_Begin : FMW_MessageType_Emergency_End); + msg.header.crc = (uint32_t)-1; + + FMW_Message response_msg = p3dx_controller->stm32_message_send(&msg); + p3dx_controller->stm32_message_print(&response_msg); assert(response_msg.header.type == FMW_MessageType_Response); response->status_response = (uint8_t)response_msg.response.result; } @@ -204,7 +216,6 @@ FMW_Message P3DX_Controller_Node::stm32_message_send(const FMW_Message *msg) assert(bytes_written != -1); assert(bytes_written > 0); assert(bytes_written == sizeof(FMW_Message)); - RCLCPP_INFO(this->get_logger(), "message sent to stm32"); for (uint32_t bytes_read = 0; bytes_read < sizeof(*msg);) { ssize_t n = ::read(this->serial_fd, ((uint8_t*)&res) + bytes_read, sizeof(res) - bytes_read); assert(n != -1); @@ -212,7 +223,6 @@ FMW_Message P3DX_Controller_Node::stm32_message_send(const FMW_Message *msg) } } this->serial_mutex.unlock(); - this->stm32_message_print(&res); assert(res.header.type == FMW_MessageType_Response); return res; } diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp index a147267..348669b 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/p3dx_node.hpp @@ -11,6 +11,7 @@ #include #include #include +#include extern "C" { #include "stm32_messages.h" @@ -22,12 +23,12 @@ enum P3DX_Cmd_ChangeMode_Kind: uint8_t { None = 0, Config = 1, Run = 2, - COUNT = 3, + Emergency = 3, + COUNT = 4, }; struct P3DX_Controller_Node: public rclcpp::Node { const int32_t serial_fd; - bool stm32_config_mode; std::mutex serial_mutex; rclcpp::Publisher::SharedPtr publisher_odometry; @@ -36,6 +37,7 @@ struct P3DX_Controller_Node: public rclcpp::Node { rclcpp::Service::SharedPtr service_config_pid; rclcpp::Service::SharedPtr service_config_robot; rclcpp::Service::SharedPtr service_config_led; + rclcpp::Service::SharedPtr service_emergency_mode; rclcpp::TimerBase::SharedPtr timer_publisher_odometry; P3DX_Controller_Node(int32_t serial_fd); @@ -50,6 +52,8 @@ struct P3DX_Controller_Node: public rclcpp::Node { std::shared_ptr response); static void callback_service_command_config_led_s(const std::shared_ptr request, std::shared_ptr response); + static void callback_service_command_emergency_mode_s(const std::shared_ptr request, + std::shared_ptr response); FMW_Message stm32_message_send(const FMW_Message *msg); void stm32_message_print(const FMW_Message *msg); diff --git a/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h b/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h index b2c08f0..e17ea98 100644 --- a/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h +++ b/pioneer_workstation_ws/src/pioneer3dx_controller/src/stm32_messages.h @@ -25,7 +25,9 @@ typedef union { X(FMW_MessageType_Config_LED, 6) \ X(FMW_MessageType_Run_GetStatus, 7) \ X(FMW_MessageType_Run_SetVelocity, 8) \ - X(FMW_MessageType_COUNT, 9) + X(FMW_MessageType_Emergency_Begin, 9) \ + X(FMW_MessageType_Emergency_End, 10) \ + X(FMW_MessageType_COUNT, 11) // NOTE(lb): Here i want to take all of the variants that are generated from the macro // expension of FMW_MESSAGE_TYPE_VARIANTS and use them to populate the enum definition.