Usm firmware#229
Conversation
removed ADC cuz no ride height, added code for imu to calculate acceleration in all three axis', added algorithm to calculate wheelspeed + can implementation.
|
The latest updates on your projects. Learn more about Vercel for GitHub.
|
There was a problem hiding this comment.
Code Review
This pull request implements the core firmware for the Upright Sensor Module, introducing new modules for IMU data acquisition, wheel speed sensing, and CAN communication alongside significant project restructuring and hardware configuration updates. The review identifies several critical issues, including a potential buffer overflow in USB string formatting, a logic error in CAN message updates that would overwrite data from other modules, and integer overflow risks in sensor calculations. Additionally, the feedback highlights the need to properly handle return values for peripheral operations to prevent silent failures and recommends removing a duplicate source file.
| */ | ||
| /* USER CODE END Header_StartDefaultTask */ | ||
| void StartDefaultTask(void *argument) |
There was a problem hiding this comment.
Potential buffer overflow and out-of-bounds read. snprintf returns the number of characters that would have been written if the buffer were large enough. If the output is truncated, len will be greater than or equal to sizeof(buf), causing CDC_Transmit_FS to read past the end of the stack-allocated buffer.
int len = snprintf(buf, sizeof(buf), "RPM:%.1f MPH:%.2f Ax:%.2f Ay:%.2f Az:%.2f\r\n",
rpm, mph, imu.accel_x, imu.accel_y, imu.accel_z);
if (len > 0) {
uint16_t transmit_len = (len >= (int)sizeof(buf)) ? (uint16_t)(sizeof(buf) - 1) : (uint16_t)len;
CDC_Transmit_FS((uint8_t *)buf, transmit_len);
}| #if defined(BOARD_FL) | ||
| wheel_speeds_mailbox.front_left_speed = wheel_speed_rads; | ||
| wheel_speeds_mailbox.front_right_speed = 0.0f; | ||
| wheel_speeds_mailbox.back_left_speed = 0.0f; | ||
| wheel_speeds_mailbox.back_right_speed = 0.0f; | ||
| #elif defined(BOARD_FR) | ||
| wheel_speeds_mailbox.front_left_speed = 0.0f; | ||
| wheel_speeds_mailbox.front_right_speed = wheel_speed_rads; | ||
| wheel_speeds_mailbox.back_left_speed = 0.0f; | ||
| wheel_speeds_mailbox.back_right_speed = 0.0f; | ||
| #elif defined(BOARD_RL) | ||
| wheel_speeds_mailbox.front_left_speed = 0.0f; | ||
| wheel_speeds_mailbox.front_right_speed = 0.0f; | ||
| wheel_speeds_mailbox.back_left_speed = wheel_speed_rads; | ||
| wheel_speeds_mailbox.back_right_speed = 0.0f; | ||
| #elif defined(BOARD_RR) | ||
| wheel_speeds_mailbox.front_left_speed = 0.0f; | ||
| wheel_speeds_mailbox.front_right_speed = 0.0f; | ||
| wheel_speeds_mailbox.back_left_speed = 0.0f; | ||
| wheel_speeds_mailbox.back_right_speed = wheel_speed_rads; | ||
| #endif |
There was a problem hiding this comment.
Logic error in unified CAN message update. By zeroing out the speeds of other corners, this board will overwrite valid data from other USMs if they share the same CAN ID (1024). Per the general rules, each message is treated as a complete state snapshot. This implementation will cause the global wheel speed state to flicker between one valid corner and three zeros.
| s->magnitude = sqrtf((float)(s->x * s->x) + | ||
| (float)(s->y * s->y) + | ||
| (float)(s->z * s->z)); |
There was a problem hiding this comment.
Potential integer overflow. s->x, s->y, and s->z are int16_t. Their squares are promoted to int32_t. The sum of three such squares can exceed the maximum value of a signed 32-bit integer (sqrtf, resulting in NaN magnitudes.
| s->magnitude = sqrtf((float)(s->x * s->x) + | |
| (float)(s->y * s->y) + | |
| (float)(s->z * s->z)); | |
| s->magnitude = sqrtf((float)s->x * s->x + | |
| (float)s->y * s->y + | |
| (float)s->z * s->z); |
| /* USER CODE BEGIN Header */ | ||
| /** | ||
| ****************************************************************************** | ||
| * @file : main.c | ||
| * @brief : Main program body | ||
| ****************************************************************************** | ||
| * @attention | ||
| * | ||
| * Copyright (c) 2025 STMicroelectronics. | ||
| * All rights reserved. | ||
| * | ||
| * This software is licensed under terms that can be found in the LICENSE file | ||
| * in the root directory of this software component. | ||
| * If no LICENSE file comes with this software, it is provided AS-IS. | ||
| * | ||
| ****************************************************************************** | ||
| */ | ||
| /* USER CODE END Header */ | ||
| /* Includes ------------------------------------------------------------------*/ | ||
| #include "main.h" | ||
| #include "cmsis_os.h" | ||
| #include "usb_device.h" | ||
|
|
||
| /* Private includes ----------------------------------------------------------*/ | ||
| /* USER CODE BEGIN Includes */ | ||
| #include "wheel_speed.h" | ||
| #include "usm_can.h" | ||
| #include "imu.h" | ||
| #include "longhorn/rtos/led.h" | ||
| #include "longhorn/rtos/logger.h" | ||
| #include "usbd_cdc_if.h" | ||
| #include <stdio.h> | ||
| /* USER CODE END Includes */ | ||
|
|
||
| /* Private typedef -----------------------------------------------------------*/ | ||
| /* USER CODE BEGIN PTD */ | ||
|
|
||
| /* USER CODE END PTD */ | ||
|
|
||
| /* Private define ------------------------------------------------------------*/ | ||
| /* USER CODE BEGIN PD */ | ||
|
|
||
| /* USER CODE END PD */ | ||
|
|
||
| /* Private macro -------------------------------------------------------------*/ | ||
| /* USER CODE BEGIN PM */ | ||
|
|
||
| /* USER CODE END PM */ | ||
|
|
||
| /* Private variables ---------------------------------------------------------*/ | ||
| FDCAN_HandleTypeDef hfdcan2; | ||
|
|
||
| SPI_HandleTypeDef hspi2; | ||
|
|
||
| TIM_HandleTypeDef htim2; | ||
|
|
||
| UART_HandleTypeDef huart1; | ||
|
|
||
| /* Definitions for defaultTask */ | ||
| osThreadId_t defaultTaskHandle; | ||
| const osThreadAttr_t defaultTask_attributes = { | ||
| .name = "defaultTask", | ||
| .priority = (osPriority_t) osPriorityNormal, | ||
| .stack_size = 512 * 4 | ||
| }; | ||
| /* USER CODE BEGIN PV */ | ||
| osThreadId_t wheelSpeedTaskHandle; | ||
| const osThreadAttr_t wheelSpeedTask_attributes = { | ||
| .name = "wheelSpeedTask", | ||
| .priority = (osPriority_t) osPriorityNormal, | ||
| .stack_size = 128 * 8 | ||
| }; | ||
| /* USER CODE END PV */ | ||
|
|
||
| /* Private function prototypes -----------------------------------------------*/ | ||
| void SystemClock_Config(void); | ||
| static void MX_GPIO_Init(void); | ||
| static void MX_FDCAN2_Init(void); | ||
| static void MX_SPI2_Init(void); | ||
| static void MX_TIM2_Init(void); | ||
| static void MX_USART1_UART_Init(void); | ||
| void StartDefaultTask(void *argument); | ||
|
|
||
| /* USER CODE BEGIN PFP */ | ||
| void StartWheelSpeedTask(void *argument); | ||
| /* USER CODE END PFP */ | ||
|
|
||
| /* Private user code ---------------------------------------------------------*/ | ||
| /* USER CODE BEGIN 0 */ | ||
|
|
||
| /* USER CODE END 0 */ | ||
|
|
||
| /** | ||
| * @brief The application entry point. | ||
| * @retval int | ||
| */ | ||
| int main(void) | ||
| { | ||
|
|
||
| /* USER CODE BEGIN 1 */ | ||
|
|
||
| /* USER CODE END 1 */ | ||
|
|
||
| /* MCU Configuration--------------------------------------------------------*/ | ||
|
|
||
| /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ | ||
| HAL_Init(); | ||
|
|
||
| /* USER CODE BEGIN Init */ | ||
|
|
||
| /* USER CODE END Init */ | ||
|
|
||
| /* Configure the system clock */ | ||
| SystemClock_Config(); | ||
|
|
||
| /* USER CODE BEGIN SysInit */ | ||
|
|
||
| /* USER CODE END SysInit */ | ||
|
|
||
| /* Initialize all configured peripherals */ | ||
| MX_GPIO_Init(); | ||
| MX_FDCAN2_Init(); | ||
| MX_SPI2_Init(); | ||
| MX_TIM2_Init(); | ||
| MX_USART1_UART_Init(); | ||
| /* USER CODE BEGIN 2 */ | ||
| /* USER CODE END 2 */ | ||
|
|
||
| /* Init scheduler */ | ||
| osKernelInitialize(); | ||
|
|
||
| /* USER CODE BEGIN RTOS_MUTEX */ | ||
| /* add mutexes, ... */ | ||
| /* USER CODE END RTOS_MUTEX */ | ||
|
|
||
| /* USER CODE BEGIN RTOS_SEMAPHORES */ | ||
| /* add semaphores, ... */ | ||
| /* USER CODE END RTOS_SEMAPHORES */ | ||
|
|
||
| /* USER CODE BEGIN RTOS_TIMERS */ | ||
| /* start timers, add new ones, ... */ | ||
| /* USER CODE END RTOS_TIMERS */ | ||
|
|
||
| /* USER CODE BEGIN RTOS_QUEUES */ | ||
| /* add queues, ... */ | ||
| /* USER CODE END RTOS_QUEUES */ | ||
|
|
||
| /* Create the thread(s) */ | ||
| /* creation of defaultTask */ | ||
| defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); | ||
|
|
||
| /* USER CODE BEGIN RTOS_THREADS */ | ||
| rainbow_led_t led = { | ||
| .ccr1 = &TIM2->CCR1, | ||
| .ccr2 = &TIM2->CCR2, | ||
| .ccr3 = &TIM2->CCR3, | ||
| .channel1 = TIM_CHANNEL_1, | ||
| .channel2 = TIM_CHANNEL_2, | ||
| .channel3 = TIM_CHANNEL_3, | ||
| .pwm_start = (HAL_PWM_Start_Fn)HAL_TIM_PWM_Start, | ||
| .timer_handle = &htim2, | ||
| }; | ||
| led_init(&led); | ||
| led_start_thread(); | ||
|
|
||
| wheelSpeedTaskHandle = osThreadNew(StartWheelSpeedTask, NULL, &wheelSpeedTask_attributes); | ||
| /* USER CODE END RTOS_THREADS */ | ||
|
|
||
| /* USER CODE BEGIN RTOS_EVENTS */ | ||
| /* add events, ... */ | ||
| /* USER CODE END RTOS_EVENTS */ | ||
|
|
||
| /* Start scheduler */ | ||
| osKernelStart(); | ||
|
|
||
| /* We should never get here as control is now taken by the scheduler */ | ||
|
|
||
| /* Infinite loop */ | ||
| /* USER CODE BEGIN WHILE */ | ||
| while (1) { | ||
| /* USER CODE END WHILE */ | ||
|
|
||
| /* USER CODE BEGIN 3 */ | ||
| } | ||
| /* USER CODE END 3 */ | ||
| } | ||
|
|
||
| /** | ||
| * @brief System Clock Configuration | ||
| * @retval None | ||
| */ | ||
| void SystemClock_Config(void) | ||
| { | ||
| RCC_OscInitTypeDef RCC_OscInitStruct = {0}; | ||
| RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; | ||
|
|
||
| /** Configure the main internal regulator output voltage | ||
| */ | ||
| HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1); | ||
|
|
||
| /** Initializes the RCC Oscillators according to the specified parameters | ||
| * in the RCC_OscInitTypeDef structure. | ||
| */ | ||
| RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; | ||
| RCC_OscInitStruct.HSIState = RCC_HSI_ON; | ||
| RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; | ||
| RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; | ||
| RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; | ||
| RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1; | ||
| RCC_OscInitStruct.PLL.PLLN = 18; | ||
| RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; | ||
| RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV6; | ||
| RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2; | ||
| if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) | ||
| { | ||
| 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_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; | ||
| RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; | ||
| RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; | ||
| RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; | ||
|
|
||
| if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * @brief FDCAN2 Initialization Function | ||
| * @param None | ||
| * @retval None | ||
| */ | ||
| static void MX_FDCAN2_Init(void) | ||
| { | ||
|
|
||
| /* USER CODE BEGIN FDCAN2_Init 0 */ | ||
|
|
||
| /* USER CODE END FDCAN2_Init 0 */ | ||
|
|
||
| /* USER CODE BEGIN FDCAN2_Init 1 */ | ||
|
|
||
| /* USER CODE END FDCAN2_Init 1 */ | ||
| hfdcan2.Instance = FDCAN2; | ||
| hfdcan2.Init.ClockDivider = FDCAN_CLOCK_DIV1; | ||
| hfdcan2.Init.FrameFormat = FDCAN_FRAME_CLASSIC; | ||
| hfdcan2.Init.Mode = FDCAN_MODE_EXTERNAL_LOOPBACK; | ||
| hfdcan2.Init.AutoRetransmission = DISABLE; | ||
| hfdcan2.Init.TransmitPause = DISABLE; | ||
| hfdcan2.Init.ProtocolException = DISABLE; | ||
| hfdcan2.Init.NominalPrescaler = 12; | ||
| hfdcan2.Init.NominalSyncJumpWidth = 1; | ||
| hfdcan2.Init.NominalTimeSeg1 = 2; | ||
| hfdcan2.Init.NominalTimeSeg2 = 1; | ||
| hfdcan2.Init.DataPrescaler = 1; | ||
| hfdcan2.Init.DataSyncJumpWidth = 1; | ||
| hfdcan2.Init.DataTimeSeg1 = 1; | ||
| hfdcan2.Init.DataTimeSeg2 = 1; | ||
| hfdcan2.Init.StdFiltersNbr = 0; | ||
| hfdcan2.Init.ExtFiltersNbr = 0; | ||
| hfdcan2.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION; | ||
| if (HAL_FDCAN_Init(&hfdcan2) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| /* USER CODE BEGIN FDCAN2_Init 2 */ | ||
|
|
||
| /* USER CODE END FDCAN2_Init 2 */ | ||
|
|
||
| } | ||
|
|
||
| /** | ||
| * @brief SPI2 Initialization Function | ||
| * @param None | ||
| * @retval None | ||
| */ | ||
| static void MX_SPI2_Init(void) | ||
| { | ||
|
|
||
| /* USER CODE BEGIN SPI2_Init 0 */ | ||
|
|
||
| /* USER CODE END SPI2_Init 0 */ | ||
|
|
||
| /* USER CODE BEGIN SPI2_Init 1 */ | ||
|
|
||
| /* USER CODE END SPI2_Init 1 */ | ||
| /* SPI2 parameter configuration*/ | ||
| hspi2.Instance = SPI2; | ||
| hspi2.Init.Mode = SPI_MODE_MASTER; | ||
| hspi2.Init.Direction = SPI_DIRECTION_2LINES; | ||
| hspi2.Init.DataSize = SPI_DATASIZE_8BIT; | ||
| hspi2.Init.CLKPolarity = SPI_POLARITY_LOW; | ||
| hspi2.Init.CLKPhase = SPI_PHASE_1EDGE; | ||
| hspi2.Init.NSS = SPI_NSS_SOFT; | ||
| hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16; | ||
| hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB; | ||
| hspi2.Init.TIMode = SPI_TIMODE_DISABLE; | ||
| hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; | ||
| hspi2.Init.CRCPolynomial = 7; | ||
| hspi2.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE; | ||
| hspi2.Init.NSSPMode = SPI_NSS_PULSE_ENABLE; | ||
| if (HAL_SPI_Init(&hspi2) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| /* USER CODE BEGIN SPI2_Init 2 */ | ||
|
|
||
| /* USER CODE END SPI2_Init 2 */ | ||
|
|
||
| } | ||
|
|
||
| /** | ||
| * @brief TIM2 Initialization Function | ||
| * @param None | ||
| * @retval None | ||
| */ | ||
| static void MX_TIM2_Init(void) | ||
| { | ||
|
|
||
| /* USER CODE BEGIN TIM2_Init 0 */ | ||
|
|
||
| /* USER CODE END TIM2_Init 0 */ | ||
|
|
||
| TIM_MasterConfigTypeDef sMasterConfig = {0}; | ||
| TIM_OC_InitTypeDef sConfigOC = {0}; | ||
|
|
||
| /* USER CODE BEGIN TIM2_Init 1 */ | ||
|
|
||
| /* USER CODE END TIM2_Init 1 */ | ||
| htim2.Instance = TIM2; | ||
| htim2.Init.Prescaler = 0; | ||
| htim2.Init.CounterMode = TIM_COUNTERMODE_UP; | ||
| htim2.Init.Period = 1000; | ||
| htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; | ||
| htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; | ||
| if (HAL_TIM_PWM_Init(&htim2) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; | ||
| sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; | ||
| if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| sConfigOC.OCMode = TIM_OCMODE_PWM1; | ||
| sConfigOC.Pulse = 0; | ||
| sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; | ||
| sConfigOC.OCFastMode = TIM_OCFAST_ENABLE; | ||
| if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| /* USER CODE BEGIN TIM2_Init 2 */ | ||
|
|
||
| /* USER CODE END TIM2_Init 2 */ | ||
| HAL_TIM_MspPostInit(&htim2); | ||
|
|
||
| } | ||
|
|
||
| /** | ||
| * @brief USART1 Initialization Function | ||
| * @param None | ||
| * @retval None | ||
| */ | ||
| static void MX_USART1_UART_Init(void) | ||
| { | ||
|
|
||
| /* USER CODE BEGIN USART1_Init 0 */ | ||
|
|
||
| /* USER CODE END USART1_Init 0 */ | ||
|
|
||
| /* USER CODE BEGIN USART1_Init 1 */ | ||
|
|
||
| /* USER CODE END USART1_Init 1 */ | ||
| huart1.Instance = USART1; | ||
| huart1.Init.BaudRate = 115200; | ||
| huart1.Init.WordLength = UART_WORDLENGTH_8B; | ||
| huart1.Init.StopBits = UART_STOPBITS_1; | ||
| huart1.Init.Parity = UART_PARITY_NONE; | ||
| huart1.Init.Mode = UART_MODE_TX_RX; | ||
| huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; | ||
| huart1.Init.OverSampling = UART_OVERSAMPLING_16; | ||
| huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; | ||
| huart1.Init.ClockPrescaler = UART_PRESCALER_DIV1; | ||
| huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; | ||
| if (HAL_UART_Init(&huart1) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| if (HAL_UARTEx_SetTxFifoThreshold(&huart1, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| if (HAL_UARTEx_SetRxFifoThreshold(&huart1, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| if (HAL_UARTEx_DisableFifoMode(&huart1) != HAL_OK) | ||
| { | ||
| Error_Handler(); | ||
| } | ||
| /* USER CODE BEGIN USART1_Init 2 */ | ||
|
|
||
| /* USER CODE END USART1_Init 2 */ | ||
|
|
||
| } | ||
|
|
||
| /** | ||
| * @brief GPIO Initialization Function | ||
| * @param None | ||
| * @retval None | ||
| */ | ||
| static void MX_GPIO_Init(void) | ||
| { | ||
| GPIO_InitTypeDef GPIO_InitStruct = {0}; | ||
| /* USER CODE BEGIN MX_GPIO_Init_1 */ | ||
|
|
||
| /* USER CODE END MX_GPIO_Init_1 */ | ||
|
|
||
| /* GPIO Ports Clock Enable */ | ||
| __HAL_RCC_GPIOC_CLK_ENABLE(); | ||
| __HAL_RCC_GPIOF_CLK_ENABLE(); | ||
| __HAL_RCC_GPIOA_CLK_ENABLE(); | ||
| __HAL_RCC_GPIOB_CLK_ENABLE(); | ||
|
|
||
| /*Configure GPIO pin Output Level */ | ||
| HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5|GPIO_PIN_6, GPIO_PIN_SET); | ||
|
|
||
| /*Configure GPIO pin Output Level */ | ||
| HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_9, GPIO_PIN_SET); | ||
|
|
||
| /*Configure GPIO pins : PC13 PC14 */ | ||
| GPIO_InitStruct.Pin = GPIO_PIN_13|GPIO_PIN_14; | ||
| GPIO_InitStruct.Mode = GPIO_MODE_INPUT; | ||
| GPIO_InitStruct.Pull = GPIO_NOPULL; | ||
| HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); | ||
|
|
||
| /*Configure GPIO pins : PA5 PA6 */ | ||
| GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6; | ||
| GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; | ||
| GPIO_InitStruct.Pull = GPIO_NOPULL; | ||
| GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; | ||
| HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); | ||
|
|
||
| /*Configure GPIO pins : PB10 PB11 PB12 PB9 */ | ||
| GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12|GPIO_PIN_9; | ||
| GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; | ||
| GPIO_InitStruct.Pull = GPIO_NOPULL; | ||
| GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; | ||
| HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); | ||
|
|
||
| /* USER CODE BEGIN MX_GPIO_Init_2 */ | ||
|
|
||
| /* USER CODE END MX_GPIO_Init_2 */ | ||
| } | ||
|
|
||
| /* USER CODE BEGIN 4 */ | ||
|
|
||
| /* USER CODE END 4 */ | ||
|
|
||
| /* USER CODE BEGIN Header_StartWheelSpeedTask */ | ||
| /** | ||
| * @brief Function implementing the wheelSpeedTask thread. | ||
| * @param argument: Not used | ||
| * @retval None | ||
| */ | ||
| /* USER CODE END Header_StartWheelSpeedTask */ | ||
| void StartWheelSpeedTask(void *argument) | ||
| { | ||
| /* USER CODE BEGIN StartWheelSpeedTask */ | ||
| osDelay(2000); // wait for USB CDC to enumerate | ||
| WheelSpeed_Init(&hspi2); | ||
| IMU_Init(&hspi2); | ||
| char buf[80]; | ||
| imu_data_t imu = {0}; | ||
| for (;;) | ||
| { | ||
| WheelSpeed_Update(); | ||
|
|
||
| // Convert RPM to rad/s and send over CAN | ||
| float rpm = WheelSpeed_GetRPM(); | ||
| float rads = rpm * 2.0f * 3.14159f / 60.0f; | ||
| float mph = WheelSpeed_GetMPH(); | ||
| usm_can_update_wheel_speed(rads); | ||
|
|
||
| // Read IMU and send over CAN | ||
| IMU_Read(&imu); | ||
| usm_can_update_accel(imu.accel_x, imu.accel_y, imu.accel_z); | ||
|
|
||
| int len = snprintf(buf, sizeof(buf), "RPM:%.1f MPH:%.2f Ax:%.2f Ay:%.2f Az:%.2f\r\n", | ||
| rpm, mph, imu.accel_x, imu.accel_y, imu.accel_z); | ||
| CDC_Transmit_FS((uint8_t *)buf, (uint16_t)len); | ||
| osDelay(200); | ||
| } | ||
| /* USER CODE END StartWheelSpeedTask */ | ||
| } | ||
|
|
||
| /* USER CODE BEGIN Header_StartDefaultTask */ | ||
| /** | ||
| * @brief Function implementing the defaultTask thread. | ||
| * @param argument: Not used | ||
| * @retval None | ||
| */ | ||
| /* USER CODE END Header_StartDefaultTask */ | ||
| void StartDefaultTask(void *argument) | ||
| { | ||
| /* init code for USB_Device */ | ||
| MX_USB_Device_Init(); | ||
| /* USER CODE BEGIN 5 */ | ||
| osDelay(500); | ||
| init_logging(CDC_Transmit_FS); | ||
| usm_can_init(); | ||
|
|
||
| /* Infinite loop */ | ||
| for(;;) | ||
| { | ||
| usm_can_debug(); | ||
| osDelay(1000); | ||
| } | ||
| /* USER CODE END 5 */ | ||
| } | ||
|
|
||
| /** | ||
| * @brief Period elapsed callback in non blocking mode | ||
| * @note This function is called when TIM20 interrupt took place, inside | ||
| * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment | ||
| * a global variable "uwTick" used as application time base. | ||
| * @param htim : TIM handle | ||
| * @retval None | ||
| */ | ||
| void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) | ||
| { | ||
| /* USER CODE BEGIN Callback 0 */ | ||
|
|
||
| /* USER CODE END Callback 0 */ | ||
| if (htim->Instance == TIM20) | ||
| { | ||
| HAL_IncTick(); | ||
| } | ||
| /* USER CODE BEGIN Callback 1 */ | ||
|
|
||
| /* USER CODE END Callback 1 */ | ||
| } | ||
|
|
||
| /** | ||
| * @brief This function is executed in case of error occurrence. | ||
| * @retval None | ||
| */ | ||
| void Error_Handler(void) | ||
| { | ||
| /* USER CODE BEGIN Error_Handler_Debug */ | ||
| /* User can add his own implementation to report the HAL error return state | ||
| */ | ||
| __disable_irq(); | ||
| while (1) { | ||
| // HAL_TIM_PWM_DeInit(&htim2); | ||
| // HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_0); | ||
| } | ||
| /* USER CODE END Error_Handler_Debug */ | ||
| } | ||
| #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 | ||
| */ | ||
| void assert_failed(uint8_t *file, uint32_t line) | ||
| { | ||
| /* USER CODE BEGIN 6 */ | ||
| /* User can add his own implementation to report the file name and line | ||
| number, ex: printf("Wrong parameters value: file %s on line %d\r\n", | ||
| file, line) */ | ||
| /* USER CODE END 6 */ | ||
| } | ||
| #endif /* USE_FULL_ASSERT */ |
|
|
||
| int len = snprintf(buf, sizeof(buf), "RPM:%.1f MPH:%.2f Ax:%.2f Ay:%.2f Az:%.2f\r\n", | ||
| rpm, mph, imu.accel_x, imu.accel_y, imu.accel_z); | ||
| CDC_Transmit_FS((uint8_t *)buf, (uint16_t)len); |
| uint8_t id = imu_read_reg(ASM330_WHO_AM_I); | ||
| if (id != ASM330_EXPECTED_ID) { | ||
| return (int)id; // returns raw ID so caller can print it for debug | ||
| } |
| usm_can_add_send_handlers(); | ||
|
|
||
| /* Start CAN interface */ | ||
| HAL_StatusTypeDef status = HAL_FDCAN_Start(&hfdcan2); |
| HAL_SPI_Transmit(_hspi, &cmd, 1, HAL_MAX_DELAY); | ||
| HAL_SPI_Receive(_hspi, rx, 9, HAL_MAX_DELAY); |
No description provided.