From: LeonardoBizzoni Date: Wed, 11 Feb 2026 10:25:54 +0000 (+0100) Subject: UART message receive only via DMA X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=056f9b25b77cc8b3012ca87ab6b96b7b08cb7284;p=pioneer-stm32 UART message receive only via DMA --- diff --git a/pioneer_controller/.settings/language.settings.xml b/pioneer_controller/.settings/language.settings.xml index a310404..a1c742c 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 3001749..74e99a4 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_core.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_core.h @@ -1,6 +1,14 @@ #ifndef FMW_CORE_H #define FMW_CORE_H +typedef uint8_t FMW_Mode; +enum { + FMW_Mode_None, + FMW_Mode_Init, + FMW_Mode_Run, + FMW_Mode_COUNT, +}; + typedef struct FMW_Encoder { TIM_HandleTypeDef *const timer; uint32_t previous_millis; @@ -74,8 +82,8 @@ typedef struct FMW_Hook { void fmw_motor_init(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); void fmw_motor_set_speed(FMW_Motor *motor, int32_t duty_cycle) __attribute__((nonnull)); void fmw_motor_brake(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); -void fmw_motor_enable(FMW_Motor * motor) __attribute__((nonnull)); -void fmw_motor_disable(FMW_Motor * motor) __attribute__((nonnull)); +void fmw_motor_enable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); +void fmw_motor_disable(FMW_Motor motors[], int32_t count) __attribute__((nonnull)); void fmw_encoder_init(FMW_Encoder encoders[], int32_t count) __attribute__((nonnull)); void fmw_encoder_update(FMW_Encoder *encoder) __attribute__((nonnull)); diff --git a/pioneer_controller/Core/Inc/firmware/fmw_messages.h b/pioneer_controller/Core/Inc/firmware/fmw_messages.h index fb65170..be39bce 100644 --- a/pioneer_controller/Core/Inc/firmware/fmw_messages.h +++ b/pioneer_controller/Core/Inc/firmware/fmw_messages.h @@ -10,16 +10,6 @@ typedef union { float values[3]; } FMW_PidConstants; -typedef uint8_t FMW_State; -enum { - FMW_State_None, - - FMW_State_Init, - FMW_State_Running, - - FMW_State_COUNT, -}; - typedef uint8_t FMW_MessageType; enum { FMW_MessageType_None, diff --git a/pioneer_controller/Core/Inc/stm32f7xx_it.h b/pioneer_controller/Core/Inc/stm32f7xx_it.h index 7fbeafb..0b4ba8b 100644 --- a/pioneer_controller/Core/Inc/stm32f7xx_it.h +++ b/pioneer_controller/Core/Inc/stm32f7xx_it.h @@ -58,7 +58,9 @@ void PendSV_Handler(void); void SysTick_Handler(void); void EXTI3_IRQHandler(void); void EXTI4_IRQHandler(void); +void DMA1_Stream1_IRQHandler(void); void ADC_IRQHandler(void); +void USART3_IRQHandler(void); void EXTI15_10_IRQHandler(void); /* USER CODE BEGIN EFP */ diff --git a/pioneer_controller/Core/Src/firmware/fwm_core.c b/pioneer_controller/Core/Src/firmware/fwm_core.c index a75c3a4..6042b5a 100644 --- a/pioneer_controller/Core/Src/firmware/fwm_core.c +++ b/pioneer_controller/Core/Src/firmware/fwm_core.c @@ -114,16 +114,34 @@ void fmw_motor_brake(FMW_Motor motors[], int32_t count) { } } -void fmw_motor_enable(FMW_Motor *motor) { - HAL_StatusTypeDef res = HAL_TIM_PWM_Start(motor->pwm_timer, motor->pwm_channel); - FMW_ASSERT(res == HAL_OK); - motor->active = true; +void fmw_motor_enable(FMW_Motor motors[], int32_t count) { + FMW_ASSERT(count > 0); + for (int32_t i = 0; i < count; ++i) { + FMW_ASSERT(motors[i].pwm_timer != NULL); + FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || + motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || + motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || + motors[i].pwm_channel == TIM_CHANNEL_ALL); + FMW_ASSERT(!motors[i].active); + HAL_StatusTypeDef res = HAL_TIM_PWM_Start(motors[i].pwm_timer, motors[i].pwm_channel); + FMW_ASSERT(res == HAL_OK); + motors[i].active = true; + } } -void fmw_motor_disable(FMW_Motor *motor) { - HAL_StatusTypeDef res = HAL_TIM_PWM_Stop(motor->pwm_timer, motor->pwm_channel); - FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); - motor->active = false; +void fmw_motor_disable(FMW_Motor motors[], int32_t count) { + FMW_ASSERT(count > 0, .callback = fmw_hook_assert_fail); + for (int32_t i = 0; i < count; ++i) { + FMW_ASSERT(motors[i].pwm_timer != NULL, .callback = fmw_hook_assert_fail); + FMW_ASSERT(motors[i].pwm_channel == TIM_CHANNEL_1 || motors[i].pwm_channel == TIM_CHANNEL_2 || + motors[i].pwm_channel == TIM_CHANNEL_3 || motors[i].pwm_channel == TIM_CHANNEL_4 || + motors[i].pwm_channel == TIM_CHANNEL_5 || motors[i].pwm_channel == TIM_CHANNEL_6 || + motors[i].pwm_channel == TIM_CHANNEL_ALL, .callback = fmw_hook_assert_fail); + FMW_ASSERT(motors[i].active); + HAL_StatusTypeDef res = HAL_TIM_PWM_Stop(motors[i].pwm_timer, motors[i].pwm_channel); + FMW_ASSERT(res == HAL_OK, .callback = fmw_hook_assert_fail); + motors[i].active = false; + } } // ============================================================ diff --git a/pioneer_controller/Core/Src/main.c b/pioneer_controller/Core/Src/main.c index 7c47c7a..1a1b1ca 100644 --- a/pioneer_controller/Core/Src/main.c +++ b/pioneer_controller/Core/Src/main.c @@ -38,12 +38,13 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ +#define MOTOR_COUNT 2 +#define ENCODER_COUNT 2 +#define UART_MESSANGER_HANDLE (&huart3) /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ -#define MOTOR_COUNT 2 -#define ENCODER_COUNT 2 /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ @@ -58,11 +59,10 @@ TIM_HandleTypeDef htim4; TIM_HandleTypeDef htim6; UART_HandleTypeDef huart3; +DMA_HandleTypeDef hdma_usart3_rx; /* USER CODE BEGIN PV */ -#define UART_MESSANGER_HANDLE (&huart3) - // TODO(lb): fill with sensible default static union { FMW_Encoder values[ENCODER_COUNT]; @@ -162,19 +162,16 @@ 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 run_msg = {0}; - -static volatile uint32_t time_aux_press = 0; -static volatile uint32_t time_aux2_press = 0; +static volatile FMW_Message uart_message_buffer = {0}; static volatile uint32_t time_last_motors = 0; -static volatile FMW_State fmw_state = FMW_State_Init; +static volatile FMW_Mode current_mode = FMW_Mode_Init; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ void SystemClock_Config(void); static void MX_GPIO_Init(void); +static void MX_DMA_Init(void); static void MX_USART3_UART_Init(void); static void MX_ADC1_Init(void); static void MX_TIM4_Init(void); @@ -191,6 +188,10 @@ static void MX_CRC_Init(void); /* USER CODE BEGIN 0 */ /* USER CODE END 0 */ +/** + * @brief The application entry point. + * @retval int + */ int main(void) { @@ -215,6 +216,7 @@ int main(void) /* Initialize all configured peripherals */ MX_GPIO_Init(); + MX_DMA_Init(); MX_USART3_UART_Init(); MX_ADC1_Init(); MX_TIM4_Init(); @@ -240,26 +242,26 @@ int main(void) } /** - * @brief System Clock Configuration - * @retval None - */ + * @brief System Clock Configuration + * @retval None + */ void SystemClock_Config(void) { RCC_OscInitTypeDef RCC_OscInitStruct = {0}; RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; /** Configure LSE Drive Capability - */ + */ HAL_PWR_EnableBkUpAccess(); /** Configure the main internal regulator output voltage - */ + */ __HAL_RCC_PWR_CLK_ENABLE(); __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3); /** Initializes the RCC Oscillators according to the specified parameters - * in the RCC_OscInitTypeDef structure. - */ + * in the RCC_OscInitTypeDef structure. + */ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; @@ -270,37 +272,37 @@ void SystemClock_Config(void) RCC_OscInitStruct.PLL.PLLQ = 4; RCC_OscInitStruct.PLL.PLLR = 2; if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /** Activate the Over-Drive mode - */ + */ if (HAL_PWREx_EnableOverDrive() != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /** Initializes the CPU, AHB and APB buses clocks - */ + */ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK - |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } } /** - * @brief ADC1 Initialization Function - * @param None - * @retval None - */ + * @brief ADC1 Initialization Function + * @param None + * @retval None + */ static void MX_ADC1_Init(void) { @@ -315,7 +317,7 @@ static void MX_ADC1_Init(void) /* USER CODE END ADC1_Init 1 */ /** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) - */ + */ hadc1.Instance = ADC1; hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; hadc1.Init.Resolution = ADC_RESOLUTION_12B; @@ -329,19 +331,19 @@ static void MX_ADC1_Init(void) hadc1.Init.DMAContinuousRequests = DISABLE; hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; if (HAL_ADC_Init(&hadc1) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. - */ + */ sConfig.Channel = ADC_CHANNEL_2; sConfig.Rank = ADC_REGULAR_RANK_1; sConfig.SamplingTime = ADC_SAMPLETIME_84CYCLES; if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN ADC1_Init 2 */ /* USER CODE END ADC1_Init 2 */ @@ -349,10 +351,10 @@ static void MX_ADC1_Init(void) } /** - * @brief CRC Initialization Function - * @param None - * @retval None - */ + * @brief CRC Initialization Function + * @param None + * @retval None + */ static void MX_CRC_Init(void) { @@ -370,9 +372,9 @@ static void MX_CRC_Init(void) hcrc.Init.OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE; hcrc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES; if (HAL_CRC_Init(&hcrc) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN CRC_Init 2 */ /* USER CODE END CRC_Init 2 */ @@ -380,10 +382,10 @@ static void MX_CRC_Init(void) } /** - * @brief TIM1 Initialization Function - * @param None - * @retval None - */ + * @brief TIM1 Initialization Function + * @param None + * @retval None + */ static void MX_TIM1_Init(void) { @@ -406,16 +408,16 @@ static void MX_TIM1_Init(void) htim1.Init.RepetitionCounter = 0; htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_PWM_Init(&htim1) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; @@ -424,13 +426,13 @@ static void MX_TIM1_Init(void) sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE; sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE; sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF; @@ -443,9 +445,9 @@ static void MX_TIM1_Init(void) sBreakDeadTimeConfig.Break2Filter = 0; sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE; if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN TIM1_Init 2 */ /* USER CODE END TIM1_Init 2 */ @@ -454,10 +456,10 @@ static void MX_TIM1_Init(void) } /** - * @brief TIM2 Initialization Function - * @param None - * @retval None - */ + * @brief TIM2 Initialization Function + * @param None + * @retval None + */ static void MX_TIM2_Init(void) { @@ -487,15 +489,15 @@ static void MX_TIM2_Init(void) sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim2, &sConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN TIM2_Init 2 */ /* USER CODE END TIM2_Init 2 */ @@ -503,10 +505,10 @@ static void MX_TIM2_Init(void) } /** - * @brief TIM3 Initialization Function - * @param None - * @retval None - */ + * @brief TIM3 Initialization Function + * @param None + * @retval None + */ static void MX_TIM3_Init(void) { @@ -536,15 +538,15 @@ static void MX_TIM3_Init(void) sConfig.IC2Prescaler = TIM_ICPSC_DIV1; sConfig.IC2Filter = 0; if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN TIM3_Init 2 */ /* USER CODE END TIM3_Init 2 */ @@ -552,10 +554,10 @@ static void MX_TIM3_Init(void) } /** - * @brief TIM4 Initialization Function - * @param None - * @retval None - */ + * @brief TIM4 Initialization Function + * @param None + * @retval None + */ static void MX_TIM4_Init(void) { @@ -576,27 +578,27 @@ static void MX_TIM4_Init(void) htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_PWM_Init(&htim4) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 0; sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN TIM4_Init 2 */ /* USER CODE END TIM4_Init 2 */ @@ -605,10 +607,10 @@ static void MX_TIM4_Init(void) } /** - * @brief TIM6 Initialization Function - * @param None - * @retval None - */ + * @brief TIM6 Initialization Function + * @param None + * @retval None + */ static void MX_TIM6_Init(void) { @@ -627,15 +629,15 @@ static void MX_TIM6_Init(void) htim6.Init.Period = 65535; htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; if (HAL_TIM_Base_Init(&htim6) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN TIM6_Init 2 */ /* USER CODE END TIM6_Init 2 */ @@ -643,10 +645,10 @@ static void MX_TIM6_Init(void) } /** - * @brief USART3 Initialization Function - * @param None - * @retval None - */ + * @brief USART3 Initialization Function + * @param None + * @retval None + */ static void MX_USART3_UART_Init(void) { @@ -668,9 +670,9 @@ static void MX_USART3_UART_Init(void) huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; if (HAL_UART_Init(&huart3) != HAL_OK) - { - Error_Handler(); - } + { + Error_Handler(); + } /* USER CODE BEGIN USART3_Init 2 */ /* USER CODE END USART3_Init 2 */ @@ -678,10 +680,26 @@ static void MX_USART3_UART_Init(void) } /** - * @brief GPIO Initialization Function - * @param None - * @retval None - */ + * Enable DMA controller clock + */ +static void MX_DMA_Init(void) +{ + + /* DMA controller clock enable */ + __HAL_RCC_DMA1_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Stream1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); + +} + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ static void MX_GPIO_Init(void) { GPIO_InitTypeDef GPIO_InitStruct = {0}; @@ -780,13 +798,9 @@ static void MX_GPIO_Init(void) /* USER CODE BEGIN 4 */ void start(void) { - for (FMW_Message msg = {0}; fmw_state == FMW_State_Init; ) { - FMW_Result res = fmw_message_uart_receive(UART_MESSANGER_HANDLE, &msg, 180 * 1000); - FMW_Message response = {0}; - response.header.type = FMW_MessageType_Response; - response.result = (res == FMW_Result_Ok) ? message_handler(&msg, &hcrc) : res; - fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); - } + // Enables UART RX interrupt + HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); + for (; current_mode == FMW_Mode_Init; ); fmw_encoder_init(encoders.values, ENCODER_COUNT); fmw_motor_init(motors.values, MOTOR_COUNT); @@ -801,9 +815,6 @@ void start(void) { HAL_StatusTypeDef timer_status = HAL_TIM_Base_Start_IT(&htim6); FMW_ASSERT(timer_status == HAL_OK); - // Enables UART RX interrupt - HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&run_msg, sizeof run_msg); - for (uint32_t time_last_led_update = 0;;) { uint32_t time_now = HAL_GetTick(); if (time_now - time_last_led_update >= led_update_period) { @@ -824,11 +835,11 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { if (!(crc_computed == crc_received)) { return FMW_Result_Error_UART_Crc; } } - switch (fmw_state) { - case FMW_State_Init: { + switch (current_mode) { + case FMW_Mode_Init: { switch (msg->header.type) { case FMW_MessageType_StateChange_Run: { - fmw_state = FMW_State_Running; + current_mode = FMW_Mode_Run; } break; case FMW_MessageType_Config_Robot: { if (!(msg->config_robot.baseline > 0.f)) { @@ -873,7 +884,7 @@ FMW_Result message_handler(FMW_Message *msg, CRC_HandleTypeDef *hcrc) { } break; } } break; - case FMW_State_Running: { + case FMW_Mode_Run: { switch (msg->header.type) { case FMW_MessageType_Run_GetStatus: { int32_t current_ticks_left = ticks_left + fmw_encoder_count_get(&encoders.left); @@ -944,69 +955,34 @@ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { if (huart == UART_MESSANGER_HANDLE) { FMW_Message response = {0}; response.header.type = FMW_MessageType_Response; - response.result = message_handler((FMW_Message*)&run_msg, &hcrc); + response.result = message_handler((FMW_Message*)&uart_message_buffer, &hcrc); fmw_message_uart_send(UART_MESSANGER_HANDLE, &hcrc, &response, HAL_MAX_DELAY); - // NOTE(lb): listen for the next message. - HAL_UART_Receive_DMA(huart, (uint8_t*)&run_msg, sizeof run_msg); + HAL_UART_Receive_DMA(UART_MESSANGER_HANDLE, (uint8_t*)&uart_message_buffer, sizeof uart_message_buffer); } } +void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { + FMW_ASSERT(false); +} + void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { uint32_t time_now = HAL_GetTick(); switch (GPIO_Pin) { - case aux_Pin: { - if (time_now - time_aux_press > FMW_DEBOUNCE_DELAY) { - // NOTE(lb): is this useful? - time_aux_press = time_now; - HAL_GPIO_TogglePin(GPIOB, LD1_Pin); - HAL_GPIO_TogglePin(GPIOB, LD2_Pin); - HAL_GPIO_TogglePin(GPIOB, LD3_Pin); - /* char msg[] = "AUX1 button pressed\r\n"; */ - /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */ - // NOTE(lb): lol strlen - } - } break; - case aux2_Pin: { - if (time_now - time_aux2_press > FMW_DEBOUNCE_DELAY) { - // NOTE(lb): is this useful? - time_aux2_press = time_now; - /* char msg[] = "AUX2 button pressed\r\n"; */ - /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */ - // NOTE(lb): lol strlen - } - } break; case motors_btn_Pin: { if (time_now - time_last_motors > FMW_DEBOUNCE_DELAY) { - // NOTE(lb): in both branches the buffer is turned off. When i should turn it on? - // even in https://github.com/giuseppe-caliaro/pioneer3dx-control - // Buzzer_Set is never called with true. - time_last_motors = time_now; - /* char msg[] = "Motors button pressed\r\n"; */ - /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */ - // NOTE(lb): lol strlen if (motors.left.active && motors.right.active) { - fmw_motor_disable(&motors.left); - fmw_motor_disable(&motors.right); + fmw_motor_disable(motors.values, ARRLENGTH(motors.values)); HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_RESET); - fmw_buzzer_set(&buzzer, 1, false); // <-------------------------------- - /* char msg[] = "Motors OFF\r\n"; */ - /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */ - // NOTE(lb): lol strlen + fmw_buzzer_set(&buzzer, 1, false); } else { - // NOTE(lb): is it safe to FMW_ASSERT here since the motors aren't running? FMW_ASSERT(!motors.left.active); FMW_ASSERT(!motors.right.active); - - fmw_motor_enable(&motors.left); - fmw_motor_enable(&motors.right); + fmw_motor_enable(motors.values, ARRLENGTH(motors.values)); HAL_GPIO_WritePin(SLED_GPIO_Port, SLED_Pin, GPIO_PIN_SET); - fmw_buzzer_set(&buzzer, 1, false); // <-------------------------------- - /* char msg[] = "Motors ON\r\n"; */ - /* HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY); */ - // NOTE(lb): lol strlen + fmw_buzzer_set(&buzzer, 1, false); } } } break; @@ -1022,9 +998,9 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { /* USER CODE END 4 */ /** - * @brief This function is executed in case of error occurrence. - * @retval None - */ + * @brief This function is executed in case of error occurrence. + * @retval None + */ void Error_Handler(void) { /* USER CODE BEGIN Error_Handler_Debug */ @@ -1033,12 +1009,12 @@ void Error_Handler(void) } #ifdef USE_FULL_ASSERT /** - * @brief Reports the name of the source file and the source line number - * where the assert_param error has occurred. - * @param file: pointer to the source file name - * @param line: assert_param error line source number - * @retval None - */ + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ void assert_failed(uint8_t *file, uint32_t line) { /* USER CODE BEGIN 6 */ diff --git a/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c b/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c index 0b7c5aa..44e3475 100644 --- a/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c +++ b/pioneer_controller/Core/Src/stm32f7xx_hal_msp.c @@ -24,6 +24,7 @@ /* USER CODE BEGIN Includes */ /* USER CODE END Includes */ +extern DMA_HandleTypeDef hdma_usart3_rx; /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN TD */ @@ -498,6 +499,28 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart) GPIO_InitStruct.Alternate = GPIO_AF7_USART3; HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); + /* USART3 DMA Init */ + /* USART3_RX Init */ + hdma_usart3_rx.Instance = DMA1_Stream1; + hdma_usart3_rx.Init.Channel = DMA_CHANNEL_4; + hdma_usart3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_usart3_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_usart3_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_usart3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_usart3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_usart3_rx.Init.Mode = DMA_NORMAL; + hdma_usart3_rx.Init.Priority = DMA_PRIORITY_LOW; + hdma_usart3_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_usart3_rx) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(huart,hdmarx,hdma_usart3_rx); + + /* USART3 interrupt Init */ + HAL_NVIC_SetPriority(USART3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(USART3_IRQn); /* USER CODE BEGIN USART3_MspInit 1 */ /* USER CODE END USART3_MspInit 1 */ @@ -528,6 +551,11 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* huart) */ HAL_GPIO_DeInit(GPIOD, STLK_RX_Pin|STLK_TX_Pin); + /* USART3 DMA DeInit */ + HAL_DMA_DeInit(huart->hdmarx); + + /* USART3 interrupt DeInit */ + HAL_NVIC_DisableIRQ(USART3_IRQn); /* USER CODE BEGIN USART3_MspDeInit 1 */ /* USER CODE END USART3_MspDeInit 1 */ diff --git a/pioneer_controller/Core/Src/stm32f7xx_it.c b/pioneer_controller/Core/Src/stm32f7xx_it.c index 04698f3..117a7c4 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 DMA_HandleTypeDef hdma_usart3_rx; +extern UART_HandleTypeDef huart3; /* USER CODE BEGIN EV */ /* USER CODE END EV */ @@ -225,6 +227,20 @@ void EXTI4_IRQHandler(void) /* USER CODE END EXTI4_IRQn 1 */ } +/** + * @brief This function handles DMA1 stream1 global interrupt. + */ +void DMA1_Stream1_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */ + + /* USER CODE END DMA1_Stream1_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart3_rx); + /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */ + + /* USER CODE END DMA1_Stream1_IRQn 1 */ +} + /** * @brief This function handles ADC1, ADC2 and ADC3 global interrupts. */ @@ -239,6 +255,20 @@ void ADC_IRQHandler(void) /* USER CODE END ADC_IRQn 1 */ } +/** + * @brief This function handles USART3 global interrupt. + */ +void USART3_IRQHandler(void) +{ + /* USER CODE BEGIN USART3_IRQn 0 */ + + /* USER CODE END USART3_IRQn 0 */ + HAL_UART_IRQHandler(&huart3); + /* USER CODE BEGIN USART3_IRQn 1 */ + + /* USER CODE END USART3_IRQn 1 */ +} + /** * @brief This function handles EXTI line[15:10] interrupts. */ diff --git a/pioneer_controller/pioneer_controller.ioc b/pioneer_controller/pioneer_controller.ioc index 219112d..ab3cea3 100644 --- a/pioneer_controller/pioneer_controller.ioc +++ b/pioneer_controller/pioneer_controller.ioc @@ -8,6 +8,18 @@ ADC1.master=1 CAD.formats= CAD.pinconfig= CAD.provider= +Dma.Request0=USART3_RX +Dma.RequestsNb=1 +Dma.USART3_RX.0.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART3_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART3_RX.0.Instance=DMA1_Stream1 +Dma.USART3_RX.0.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART3_RX.0.MemInc=DMA_MINC_ENABLE +Dma.USART3_RX.0.Mode=DMA_NORMAL +Dma.USART3_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART3_RX.0.PeriphInc=DMA_PINC_DISABLE +Dma.USART3_RX.0.Priority=DMA_PRIORITY_LOW +Dma.USART3_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode File.Version=6 GPIO.groupedBy=Group By Peripherals KeepUserPlacement=false @@ -15,17 +27,18 @@ Mcu.CPN=STM32F767ZIT6 Mcu.Family=STM32F7 Mcu.IP0=ADC1 Mcu.IP1=CORTEX_M7 -Mcu.IP10=TIM6 -Mcu.IP11=USART3 +Mcu.IP10=TIM4 +Mcu.IP11=TIM6 +Mcu.IP12=USART3 Mcu.IP2=CRC -Mcu.IP3=NVIC -Mcu.IP4=RCC -Mcu.IP5=SYS -Mcu.IP6=TIM1 -Mcu.IP7=TIM2 -Mcu.IP8=TIM3 -Mcu.IP9=TIM4 -Mcu.IPNb=12 +Mcu.IP3=DMA +Mcu.IP4=NVIC +Mcu.IP5=RCC +Mcu.IP6=SYS +Mcu.IP7=TIM1 +Mcu.IP8=TIM2 +Mcu.IP9=TIM3 +Mcu.IPNb=13 Mcu.Name=STM32F767ZITx Mcu.Package=LQFP144 Mcu.Pin0=PC13 @@ -68,6 +81,7 @@ MxCube.Version=6.15.0 MxDb.Version=DB.6.0.150 NVIC.ADC_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false +NVIC.DMA1_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:false NVIC.EXTI15_10_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true NVIC.EXTI3_IRQn=true\:0\:0\:false\:false\:true\:true\:true\:true @@ -80,6 +94,7 @@ 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.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 PA0/WKUP.GPIO_Label=REA @@ -228,7 +243,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,3-MX_USART3_UART_Init-USART3-false-HAL-true,4-MX_ADC1_Init-ADC1-false-HAL-true,5-MX_TIM4_Init-TIM4-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_TIM2_Init-TIM2-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_TIM6_Init-TIM6-false-HAL-true,0-MX_CORTEX_M7_Init-CORTEX_M7-false-HAL-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 RCC.48MHZClocksFreq_Value=24000000 RCC.ADC12outputFreq_Value=72000000 RCC.ADC34outputFreq_Value=72000000