diff --git a/stm32-tests/imu-uart/Core/Inc/mpu9250.h b/stm32-tests/imu-uart/Core/Inc/mpu9250.h index 998b0fe..96ad7f0 100644 --- a/stm32-tests/imu-uart/Core/Inc/mpu9250.h +++ b/stm32-tests/imu-uart/Core/Inc/mpu9250.h @@ -11,25 +11,25 @@ #include "stm32f4xx_hal.h" #include -#define DEVICE_ADDRESS 0b1101000 -#define MPU9250_ADDRESS (DEVICE_ADDRESS << 1) +#define DEVICE_ADDRESS 0b1101000 +#define MPU9250_ADDRESS (DEVICE_ADDRESS << 1) -#define FS_GYRO_250 0b00000000 -#define FS_GYRO_500 0b00001000 -#define FS_GYRO_1000 0b00010000 -#define FS_GYRO_2000 0b00011000 +#define FS_GYRO_250 0b00000000 +#define FS_GYRO_500 0b00001000 +#define FS_GYRO_1000 0b00010000 +#define FS_GYRO_2000 0b00011000 -#define FS_ACCEL_2G 0b00000000 -#define FS_ACCEL_4G 0b00001000 -#define FS_ACCEL_8G 0b00010000 -#define FS_ACCEL_16G 0b00011000 +#define FS_ACCEL_2G 0b00000000 +#define FS_ACCEL_4G 0b00001000 +#define FS_ACCEL_8G 0b00010000 +#define FS_ACCEL_16G 0b00011000 -#define REG_CONFIG_GYRO 27 -#define REG_CONFIG_ACCEL 28 -#define REG_POW_MAN 107 +#define REG_CONFIG_GYRO 27 +#define REG_CONFIG_ACCEL 28 +#define REG_POW_MAN 107 -#define REG_ACCEL_DATA 59 -#define REG_GYRO_DATA 67 +#define REG_ACCEL_DATA 59 +#define REG_GYRO_DATA 67 typedef enum { @@ -60,8 +60,9 @@ typedef struct void mpu9250_init_driver(I2C_HandleTypeDef *hi2c); uint8_t mpu9250_check_connection(void); +void mpu9250_calibrate(void); uint8_t mpu9250_configure(void); uint8_t mpu9250_read(void); -MPU9250_Data_t* mpu9250_get_data(void); +MPU9250_Data_t *mpu9250_get_data(void); #endif /* INC_MPU9250_H_ */ diff --git a/stm32-tests/imu-uart/Core/Src/main.c b/stm32-tests/imu-uart/Core/Src/main.c index dc80544..ba294e7 100644 --- a/stm32-tests/imu-uart/Core/Src/main.c +++ b/stm32-tests/imu-uart/Core/Src/main.c @@ -1,20 +1,20 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file : main.c - * @brief : Main program body - ****************************************************************************** - * @attention - * - * Copyright (c) 2026 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. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 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" @@ -69,320 +69,326 @@ static void process_command(void); /* USER CODE END 0 */ /** - * @brief The application entry point. - * @retval int - */ + * @brief The application entry point. + * @retval int + */ int main(void) { - /* USER CODE BEGIN 1 */ + /* USER CODE BEGIN 1 */ - /* USER CODE END 1 */ + /* USER CODE END 1 */ - /* MCU Configuration--------------------------------------------------------*/ + /* MCU Configuration--------------------------------------------------------*/ - /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ - HAL_Init(); + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); - /* USER CODE BEGIN Init */ + /* USER CODE BEGIN Init */ - /* USER CODE END Init */ + /* USER CODE END Init */ - /* Configure the system clock */ - SystemClock_Config(); + /* Configure the system clock */ + SystemClock_Config(); - /* USER CODE BEGIN SysInit */ + /* USER CODE BEGIN SysInit */ - /* USER CODE END SysInit */ + /* USER CODE END SysInit */ - /* Initialize all configured peripherals */ - MX_GPIO_Init(); - MX_USART2_UART_Init(); - MX_I2C3_Init(); - /* USER CODE BEGIN 2 */ - mpu9250_init_driver(&hi2c3); - HAL_UART_Receive_IT(&huart2, rx_data, 1); - /* USER CODE END 2 */ + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + MX_USART2_UART_Init(); + MX_I2C3_Init(); + /* USER CODE BEGIN 2 */ + mpu9250_init_driver(&hi2c3); + HAL_UART_Receive_IT(&huart2, rx_data, 1); - /* Infinite loop */ - /* USER CODE BEGIN WHILE */ - while (1) - { - /* USER CODE END WHILE */ + if (mpu9250_configure()) { + printf("MPU9250 Configured"); + } - /* USER CODE BEGIN 3 */ + // 300 ms + mpu9250_calibrate(); + printf("MPU9250 Calibrated"); + + /* USER CODE END 2 */ - // ALWAYS read values - mpu9250_read(); + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + /* USER CODE END WHILE */ - if (cmd_ready) - { - cmd_ready = 0; - process_command(); - } + /* USER CODE BEGIN 3 */ - // Add a small delay - HAL_Delay(10); - } - /* USER CODE END 3 */ + // ALWAYS read values + mpu9250_read(); + + if (cmd_ready) + { + cmd_ready = 0; + process_command(); + } + + // Add a small delay + HAL_Delay(10); + } + /* USER CODE END 3 */ } /** - * @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 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. - */ - 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 = 16; - RCC_OscInitStruct.PLL.PLLN = 336; - RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; - RCC_OscInitStruct.PLL.PLLQ = 2; - RCC_OscInitStruct.PLL.PLLR = 2; - 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_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) - { - Error_Handler(); - } + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** 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. + */ + 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 = 16; + RCC_OscInitStruct.PLL.PLLN = 336; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; + RCC_OscInitStruct.PLL.PLLQ = 2; + RCC_OscInitStruct.PLL.PLLR = 2; + 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_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) + { + Error_Handler(); + } } /** - * @brief I2C3 Initialization Function - * @param None - * @retval None - */ + * @brief I2C3 Initialization Function + * @param None + * @retval None + */ static void MX_I2C3_Init(void) { - /* USER CODE BEGIN I2C3_Init 0 */ - - /* USER CODE END I2C3_Init 0 */ - - /* USER CODE BEGIN I2C3_Init 1 */ - - /* USER CODE END I2C3_Init 1 */ - hi2c3.Instance = I2C3; - hi2c3.Init.ClockSpeed = 100000; - hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2; - hi2c3.Init.OwnAddress1 = 0; - hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; - hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; - hi2c3.Init.OwnAddress2 = 0; - hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; - hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; - if (HAL_I2C_Init(&hi2c3) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN I2C3_Init 2 */ - - /* USER CODE END I2C3_Init 2 */ - + /* USER CODE BEGIN I2C3_Init 0 */ + + /* USER CODE END I2C3_Init 0 */ + + /* USER CODE BEGIN I2C3_Init 1 */ + + /* USER CODE END I2C3_Init 1 */ + hi2c3.Instance = I2C3; + hi2c3.Init.ClockSpeed = 100000; + hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2; + hi2c3.Init.OwnAddress1 = 0; + hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + hi2c3.Init.OwnAddress2 = 0; + hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + if (HAL_I2C_Init(&hi2c3) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN I2C3_Init 2 */ + + /* USER CODE END I2C3_Init 2 */ } /** - * @brief USART2 Initialization Function - * @param None - * @retval None - */ + * @brief USART2 Initialization Function + * @param None + * @retval None + */ static void MX_USART2_UART_Init(void) { - /* USER CODE BEGIN USART2_Init 0 */ - - /* USER CODE END USART2_Init 0 */ + /* USER CODE BEGIN USART2_Init 0 */ - /* USER CODE BEGIN USART2_Init 1 */ + /* USER CODE END USART2_Init 0 */ - /* USER CODE END USART2_Init 1 */ - huart2.Instance = USART2; - huart2.Init.BaudRate = 115200; - huart2.Init.WordLength = UART_WORDLENGTH_8B; - huart2.Init.StopBits = UART_STOPBITS_1; - huart2.Init.Parity = UART_PARITY_NONE; - huart2.Init.Mode = UART_MODE_TX_RX; - huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart2.Init.OverSampling = UART_OVERSAMPLING_16; - if (HAL_UART_Init(&huart2) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN USART2_Init 2 */ + /* USER CODE BEGIN USART2_Init 1 */ - /* USER CODE END USART2_Init 2 */ + /* USER CODE END USART2_Init 1 */ + huart2.Instance = USART2; + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART2_Init 2 */ + /* USER CODE END USART2_Init 2 */ } /** - * @brief GPIO Initialization Function - * @param None - * @retval None - */ + * @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 */ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + /* USER CODE BEGIN MX_GPIO_Init_1 */ - /* USER CODE END MX_GPIO_Init_1 */ + /* USER CODE END MX_GPIO_Init_1 */ - /* GPIO Ports Clock Enable */ - __HAL_RCC_GPIOC_CLK_ENABLE(); - __HAL_RCC_GPIOH_CLK_ENABLE(); - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOH_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); - /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET); + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET); - /*Configure GPIO pin : B1_Pin */ - GPIO_InitStruct.Pin = B1_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : B1_Pin */ + GPIO_InitStruct.Pin = B1_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct); - /*Configure GPIO pin : LD2_Pin */ - GPIO_InitStruct.Pin = LD2_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : LD2_Pin */ + GPIO_InitStruct.Pin = LD2_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct); - /* USER CODE BEGIN MX_GPIO_Init_2 */ + /* USER CODE BEGIN MX_GPIO_Init_2 */ - /* USER CODE END MX_GPIO_Init_2 */ + /* USER CODE END MX_GPIO_Init_2 */ } /* USER CODE BEGIN 4 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - UNUSED(huart); - - if (rx_data[0] == '\n' || rx_data[0] == '\r') - { - if (rx_index > 0) - { - rx_buffer[rx_index] = '\0'; - cmd_ready = 1; - rx_index = 0; - } - } - else if (rx_index < sizeof(rx_buffer) - 1) - { - rx_buffer[rx_index++] = rx_data[0]; - } - - HAL_UART_Receive_IT(&huart2, rx_data, 1); + UNUSED(huart); + + if (rx_data[0] == '\n' || rx_data[0] == '\r') + { + if (rx_index > 0) + { + rx_buffer[rx_index] = '\0'; + cmd_ready = 1; + rx_index = 0; + } + } + else if (rx_index < sizeof(rx_buffer) - 1) + { + rx_buffer[rx_index++] = rx_data[0]; + } + + HAL_UART_Receive_IT(&huart2, rx_data, 1); } static void process_command(void) { - MPU9250_Data_t *imu = mpu9250_get_data(); - uint16_t len; - - if (strcmp((char*)rx_buffer, "READ") == 0) - { - - len = sprintf((char*)tx_buffer, - "AX:%.2f AY:%.2f AZ:%.2f GX:%.2f GY:%.2f GZ:%.2f\r\n", - imu->accel.filt_x, imu->accel.filt_y, imu->accel.filt_z, - imu->gyro.filt_x, imu->gyro.filt_y, imu->gyro.filt_z); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "STATUS") == 0) - { - mpu9250_check_connection(); - if (imu->state == SENSOR_STATE_CONNECTED) - { - len = sprintf((char*)tx_buffer, "STATUS:CONNECTED\r\n"); - } - else - { - len = sprintf((char*)tx_buffer, "STATUS:LOST\r\n"); - } - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "REGISTER") == 0) - { - len = sprintf((char*)tx_buffer, "REGISTER:0x%02X\r\n", DEVICE_ADDRESS); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "CONFIG") == 0) - { - len = sprintf((char*)tx_buffer, - "GYRO_CFG:0x%02X ACCEL_CFG:0x%02X\r\n", - imu->gyro_config, imu->accel_config); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "POWER") == 0) - { - len = sprintf((char*)tx_buffer, "POWER_CFG:0x%02X\r\n", imu->power_config); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else - { - len = sprintf((char*)tx_buffer, "ERR:UNKNOWN_CMD\r\n"); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } + MPU9250_Data_t *imu = mpu9250_get_data(); + uint16_t len; + + if (strcmp((char *)rx_buffer, "READ") == 0) + { + + len = sprintf((char *)tx_buffer, + "AX:%.2f AY:%.2f AZ:%.2f GX:%.2f GY:%.2f GZ:%.2f\r\n", + imu->accel.filt_x, imu->accel.filt_y, imu->accel.filt_z, + imu->gyro.filt_x, imu->gyro.filt_y, imu->gyro.filt_z); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "STATUS") == 0) + { + mpu9250_check_connection(); + if (imu->state == SENSOR_STATE_CONNECTED) + { + len = sprintf((char *)tx_buffer, "STATUS:CONNECTED\r\n"); + } + else + { + len = sprintf((char *)tx_buffer, "STATUS:LOST\r\n"); + } + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "REGISTER") == 0) + { + len = sprintf((char *)tx_buffer, "REGISTER:0x%02X\r\n", DEVICE_ADDRESS); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "CONFIG") == 0) + { + len = sprintf((char *)tx_buffer, + "GYRO_CFG:0x%02X ACCEL_CFG:0x%02X\r\n", + imu->gyro_config, imu->accel_config); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "POWER") == 0) + { + len = sprintf((char *)tx_buffer, "POWER_CFG:0x%02X\r\n", imu->power_config); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else + { + len = sprintf((char *)tx_buffer, "ERR:UNKNOWN_CMD\r\n"); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } } /* 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 */ - /* User can add his own implementation to report the HAL error return state */ - __disable_irq(); - while (1) - { - } - /* USER CODE END Error_Handler_Debug */ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* 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 - */ + * @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 */ + /* 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 */ \ No newline at end of file diff --git a/stm32-tests/imu-uart/Core/Src/mpu9250.c b/stm32-tests/imu-uart/Core/Src/mpu9250.c index dc4eb64..9f77897 100644 --- a/stm32-tests/imu-uart/Core/Src/mpu9250.c +++ b/stm32-tests/imu-uart/Core/Src/mpu9250.c @@ -9,6 +9,13 @@ #include "main.h" #include +// https://invensense.tdk.com/wp-content/uploads/2017/11/RM-MPU-9250A-00-v1.6.pdf +#define REG_CONFIG_GYRO_DLPF 26 // 0x1A (Configuration + Gyroscope DLPF) +#define REG_CONFIG_GYRO 27 // 0x1B (Gyroscope Configuration) +#define REG_CONFIG_ACCEL 28 // 0x1C (Accelerometer Configuration) +#define REG_CONFIG_ACCEL_2 29 // 0x1D (Accelerometer DLPF Configuration) +#define REG_POW_MAN 107 // 0x6B (Power Management 1) + #define GRAVITY 9.80665f // https://www.st.com/resource/en/datasheet/lsm6ds3tr-c.pdf @@ -45,38 +52,96 @@ uint8_t mpu9250_check_connection(void) if (ret == HAL_OK) { imu_data.state = SENSOR_STATE_CONNECTED; + return 1; } imu_data.state = SENSOR_STATE_LOST; + return 0; } +void mpu9250_calibrate(void) { + if (!calibrated) + { + uint8_t data[14]; + + long total_gx_raw = 0; + long total_gy_raw = 0; + long total_gz_raw = 0; + + int sample_num = 100; + + for (int i = 0; i < sample_num; ++i) + { + HAL_I2C_Mem_Read(_hi2c, MPU9250_ADDRESS, REG_ACCEL_DATA, 1, data, 14, 100); + + total_gx += ((int16_t)data[8] << 8) + data[9]; + total_gy += ((int16_t)data[10] << 8) + data[11]; + total_gz += ((int16_t)data[12] << 8) + data[13]; + + HAL_Delay(3); + } + + offset_gx = (float)(total_gx / sample_num) * ANG_VEL_SENSITIVITY_500DPS; + offset_gy = (float)(total_gy / sample_num) * ANG_VEL_SENSITIVITY_500DPS; + offset_gz = (float)(total_gz / sample_num) * ANG_VEL_SENSITIVITY_500DPS; + + // TODO: Add accelerometer calibration, otherwise assume factory + + offset_ax = 0; + offset_ay = 0; + offset_az = 0; + + calibrated = 1; + } +} + uint8_t mpu9250_configure(void) { uint8_t temp_data; HAL_StatusTypeDef ret; - // Configure Accelerometer (4G, matching discovery implementation) - temp_data = FS_ACCEL_4G; - ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_CONFIG_ACCEL, 1, &temp_data, 1, 100); + // Configure Power + temp_data = 0x00; + ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_POW_MAN, 1, &temp_data, 1, 100); + if (ret != HAL_OK) return 0; - imu_data.accel_config = temp_data; - // Configure Gyroscope (500dps, matching discovery implementation) - temp_data = FS_GYRO_500; + imu_data.power_config = temp_data; + + // Configure Gyroscope DLPF - Register 26 (0x1A) + temp_data = 0x03; + ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_CONFIG_GYRO_DLPF, 1, &temp_data, 1, 100); + + if (ret != HAL_OK) + return 0; + + // Configure Gyroscope - Register 27 (0x1B) + temp_data = (1 << 3); ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_CONFIG_GYRO, 1, &temp_data, 1, 100); + if (ret != HAL_OK) return 0; + imu_data.gyro_config = temp_data; - // Configure Power (wake up sensor) - temp_data = 0x00; - ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_POW_MAN, 1, &temp_data, 1, 100); + // Configure Accelerometer DLPF - Register 29 (0x1D) + temp_data = 0x03; + ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_CONFIG_ACCEL_2, 1, &temp_data, 1, 100); + + if (ret != HAL_OK) + return 0; + + imu_data.accel_config = temp_data; + + // Configure Accelerometer - Register 28 (0x1C) + temp_data = (1 << 3); + ret = HAL_I2C_Mem_Write(_hi2c, MPU9250_ADDRESS, REG_CONFIG_ACCEL, 1, &temp_data, 1, 100); + if (ret != HAL_OK) return 0; - imu_data.power_config = temp_data; return 1; } @@ -101,6 +166,7 @@ uint8_t mpu9250_read(void) if (ret != HAL_OK) { imu_data.state = SENSOR_STATE_LOST; + return 0; } @@ -116,54 +182,11 @@ uint8_t mpu9250_read(void) x_accel = ((int16_t)data[0] << 8) + data[1]; y_accel = ((int16_t)data[2] << 8) + data[3]; z_accel = ((int16_t)data[4] << 8) + data[5]; + x_gyro = ((int16_t)data[8] << 8) + data[9]; y_gyro = ((int16_t)data[10] << 8) + data[11]; z_gyro = ((int16_t)data[12] << 8) + data[13]; - // Calibrate IMU by calculating offset - if (!calibrated) - { - - float total_off_gx = 0, total_off_gy = 0, total_off_gz = 0; - float total_off_ax = 0, total_off_ay = 0, total_off_az = 0; - - int sample_num = 100; - - for(int i = 0; i -#define DEVICE_ADDRESS 0b1101010 // SA0 pin LOW -#define LSM6DS3TR_ADDRESS (DEVICE_ADDRESS << 1) +#define DEVICE_ADDRESS 0b1101010 // SA0 pin LOW +#define LSM6DS3TR_ADDRESS (DEVICE_ADDRESS << 1) // Scale selections (for configuration) -#define FS_GYRO_500DPS 0b00000100 // 0x04 -> Table 54. of datasheet -#define FS_ACCEL_4G 0b00001000 // 0x08 -> Table 51. of datasheet +#define FS_GYRO_500DPS 0b00000100 // 0x04 -> Table 54. of datasheet +#define FS_ACCEL_4G 0b00001000 // 0x08 -> Table 51. of datasheet // Check connection (lsm6ds3tr_check_connection) -#define REG_WHO_AM_I 0x0F -#define WHO_AM_I_VAL 0x6A // expected WHO_AM_I response for LSM6DS3TR-C (verify) +#define REG_WHO_AM_I 0x0F +#define WHO_AM_I_VAL 0x6A // expected WHO_AM_I response for LSM6DS3TR-C (verify) // Control registers -#define REG_CTRL1_XL 0x10 // Accelerometer control register -#define REG_CTRL2_G 0x11 // Gyroscope control register +#define REG_CTRL1_XL 0x10 // Accelerometer control register +#define REG_CTRL2_G 0x11 // Gyroscope control register +#define REG_CTRL3_C 0x12 // BDU, Reset // #define REG_POW_MAN 107 (come back to this after rewriting .c) // Output registers -#define REG_OUTX_L_G 0x22 // gyro x-axis LB (starts at 0x22 and goes to 0x27 for gyro) -#define REG_OUTX_L_XL 0x28 // accel x-axis LB (starts at 0x28 and goes to 0x2D for accel) +#define REG_OUTX_L_G 0x22 // gyro x-axis LB (starts at 0x22 and goes to 0x27 for gyro) +#define REG_OUTX_L_XL 0x28 // accel x-axis LB (starts at 0x28 and goes to 0x2D for accel) +#define BDU_ENABLE 0x44 // Block Data Update = 1, IF_INC = 1 typedef enum { @@ -61,7 +63,8 @@ typedef struct void lsm6ds3tr_init_driver(I2C_HandleTypeDef *hi2c); uint8_t lsm6ds3tr_check_connection(void); uint8_t lsm6ds3tr_configure(void); +void lsm6ds3tr_calibrate(void); uint8_t lsm6ds3tr_read(void); -LSM6DS3TR_Data_t* lsm6ds3tr_get_data(void); +LSM6DS3TR_Data_t *lsm6ds3tr_get_data(void); #endif /* INC_LSM6DS3TR_H_ */ \ No newline at end of file diff --git a/stm32-tests/sensor-testing/Core/Src/lsm6ds3tr.c b/stm32-tests/sensor-testing/Core/Src/lsm6ds3tr.c index 3c86749..d7be91d 100644 --- a/stm32-tests/sensor-testing/Core/Src/lsm6ds3tr.c +++ b/stm32-tests/sensor-testing/Core/Src/lsm6ds3tr.c @@ -21,6 +21,8 @@ static I2C_HandleTypeDef *_hi2c; static LSM6DS3TR_Data_t imu_data; +static uint8_t dma_rx_buffer[12]; + // Variable definitions for offset calculation static uint16_t calibrated = 0; static float offset_gx = 0, offset_gy = 0, offset_gz = 0; @@ -32,10 +34,18 @@ void lsm6ds3tr_init_driver(I2C_HandleTypeDef *hi2c) imu_data.state = SENSOR_STATE_LOST; // Initialize struct with default values - imu_data.accel.x = 0; imu_data.accel.y = 0; imu_data.accel.z = 0; - imu_data.gyro.x = 0; imu_data.gyro.y = 0; imu_data.gyro.z = 0; - imu_data.accel.filt_x = 0; imu_data.accel.filt_y = 0; imu_data.accel.filt_z = 0; - imu_data.gyro.filt_x = 0; imu_data.gyro.filt_y = 0; imu_data.gyro.filt_z = 0; + imu_data.accel.x = 0; + imu_data.accel.y = 0; + imu_data.accel.z = 0; + imu_data.gyro.x = 0; + imu_data.gyro.y = 0; + imu_data.gyro.z = 0; + imu_data.accel.filt_x = 0; + imu_data.accel.filt_y = 0; + imu_data.accel.filt_z = 0; + imu_data.gyro.filt_x = 0; + imu_data.gyro.filt_y = 0; + imu_data.gyro.filt_z = 0; // Initialize config values to 0 (not configured) imu_data.gyro_config = 0; @@ -53,10 +63,12 @@ uint8_t lsm6ds3tr_check_connection(void) if (ret == HAL_OK && who_am_i == WHO_AM_I_VAL) { imu_data.state = SENSOR_STATE_CONNECTED; + return 1; } imu_data.state = SENSOR_STATE_LOST; + return 0; } @@ -68,15 +80,73 @@ uint8_t lsm6ds3tr_configure(void) // Turn on Accelerometer -> configure accel (104 Hz ODR, 4g FS) temp_data = ODR_104HZ | FS_ACCEL_4G; ret = HAL_I2C_Mem_Write(_hi2c, LSM6DS3TR_ADDRESS, REG_CTRL1_XL, 1, &temp_data, 1, 100); - if (ret != HAL_OK) return 0; + + if (ret != HAL_OK) + return 0; + imu_data.accel_config = temp_data; // Turn on Gyroscope -> configure accel (104 Hz ODR, 500dps FS) temp_data = ODR_104HZ | FS_GYRO_500DPS; ret = HAL_I2C_Mem_Write(_hi2c, LSM6DS3TR_ADDRESS, REG_CTRL2_G, 1, &temp_data, 1, 100); - if (ret != HAL_OK) return 0; + + if (ret != HAL_OK) + return 0; + imu_data.gyro_config = temp_data; + // Enable Block Data Update + temp_data = BDU_ENABLE; + ret = HAL_I2C_Mem_Write(_hi2c, LSM6DS3TR_ADDRESS, REG_CTRL3_C, 1, &temp_data, 1, 100); + + if (ret != HAL_OK) + return 0; + + return 1; +} + +void lsm6ds3tr_calibrate(void) +{ + HAL_StatusTypeDef ret; + + uint8_t data[12]; + + long total_off_gx = 0; + long total_off_gy = 0; + long total_off_gz = 0; + + long total_off_ax = 0; + long total_off_ay = 0; + long total_off_az = 0; + + for (int i = 0; i < 100; ++i) + { + HAL_I2C_Mem_Read(_hi2c, LSM6DS3TR_ADDRESS, REG_OUTX_L_G, 1, data, 12, 100); + + int16_t curr_off_gx = ((int16_t)data[1] << 8) + data[0]; + int16_t curr_off_gy = ((int16_t)data[3] << 8) + data[2]; + int16_t curr_off_gz = ((int16_t)data[5] << 8) + data[4]; + + int16_t curr_off_gx = ((int16_t)data[7] << 8) + data[6]; + int16_t curr_off_gy = ((int16_t)data[9] << 8) + data[8]; + int16_t curr_off_gz = ((int16_t)data[11] << 8) + data[10]; + + total_off_gx += curr_off_gx; + total_off_gy += curr_off_gy; + total_off_gz += curr_off_gz; + + HAL_Delay(3); + } + + // TODO: Add Accelerometer calibration + offset_ax = 0; + offset_ay = 0; + offset_az = 0; + + offset_gx = (total_off_gx / 100) * ANG_VEL_SENSITIVITY_500DPS; + offset_gy = (total_off_gy / 100) * ANG_VEL_SENSITIVITY_500DPS; + offset_gz = (total_off_gz / 100) * ANG_VEL_SENSITIVITY_500DPS; + return 1; } @@ -91,24 +161,13 @@ uint8_t lsm6ds3tr_read(void) return 0; } - // 6-byte buffers for accel and gyro - HAL_StatusTypeDef ret; - uint8_t accel_buffer[6]; - uint8_t gyro_buffer[6]; + uint8_t buffer[12]; + HAL_StatusTypeDef ret = HAL_I2C_Mem_Read(_hi2c, LSM6DS3TR_ADDRESS, REG_OUTX_L_G, 1, buffer, 12, 100); - // Read accel data (6 bytes starting at REG_OUTX_L_XL [0x28], incrementing up to 0x2D) - ret = HAL_I2C_Mem_Read(_hi2c, LSM6DS3TR_ADDRESS, REG_OUTX_L_XL, 1, accel_buffer, 6, 100); if (ret != HAL_OK) { imu_data.state = SENSOR_STATE_LOST; - return 0; - } - // Read gyro data (6 bytes starting at REG_OUTX_L_G [0x22], incrementing up to 0x27) - ret = HAL_I2C_Mem_Read(_hi2c, LSM6DS3TR_ADDRESS, REG_OUTX_L_G, 1, gyro_buffer, 6, 100); - if (ret != HAL_OK) - { - imu_data.state = SENSOR_STATE_LOST; return 0; } @@ -121,58 +180,13 @@ uint8_t lsm6ds3tr_read(void) int16_t z_gyro; // Get raw data from IMU (LITTLE_ENDIAN, so LB comes before HB [opposite of mpu9250]) - x_accel = ((int16_t)accel_buffer[1] << 8) + accel_buffer[0]; - y_accel = ((int16_t)accel_buffer[3] << 8) + accel_buffer[2]; - z_accel = ((int16_t)accel_buffer[5] << 8) + accel_buffer[4]; - - x_gyro = ((int16_t)gyro_buffer[1] << 8) + gyro_buffer[0]; - y_gyro = ((int16_t)gyro_buffer[3] << 8) + gyro_buffer[2]; - z_gyro = ((int16_t)gyro_buffer[5] << 8) + gyro_buffer[4]; + x_accel = ((int16_t)buffer[1] << 8) + buffer[0]; + y_accel = ((int16_t)buffer[3] << 8) + buffer[2]; + z_accel = ((int16_t)buffer[5] << 8) + buffer[4]; - // Calibrate IMU by calculating offsets (only occurs during setup, & assumes stationary during calibration) - if (!calibrated) - { - - float total_off_gx = 0, total_off_gy = 0, total_off_gz = 0; - float total_off_ax = 0, total_off_ay = 0, total_off_az = 0; - - int sample_num = 100; - - for(int i = 0; iInstance == _hi2c->Instance) + { + // Buffer Layout: [0 - 5] -> Gyroscope, [6 - 11] -> Accelerometer + + int16_t x_gyro = ((int16_t)dma_rx_buffer[1] << 8) + dma_rx_buffer[0]; + int16_t y_gyro = ((int16_t)dma_rx_buffer[3] << 8) + dma_rx_buffer[2]; + int16_t z_gyro = ((int16_t)dma_rx_buffer[5] << 8) + dma_rx_buffer[4]; + + int16_t x_accel = ((int16_t)dma_rx_buffer[7] << 8) + dma_rx_buffer[6]; + int16_t y_accel = ((int16_t)dma_rx_buffer[9] << 8) + dma_rx_buffer[8]; + int16_t z_accel = ((int16_t)dma_rx_buffer[11] << 8) + dma_rx_buffer[10]; + + // Unit Conversions + Offset Application (MATCHING THE READ FUNCTION) + float x_accel_ms2 = (x_accel * LIN_ACCEL_SENSITIVITY_4G) * GRAVITY - offset_ax; + float y_accel_ms2 = (y_accel * LIN_ACCEL_SENSITIVITY_4G) * GRAVITY - offset_ay; + float z_accel_ms2 = (z_accel * LIN_ACCEL_SENSITIVITY_4G) * GRAVITY + offset_az; + + float x_gyro_dps = (x_gyro * ANG_VEL_SENSITIVITY_500DPS) - offset_gx; + float y_gyro_dps = (y_gyro * ANG_VEL_SENSITIVITY_500DPS) - offset_gy; + float z_gyro_dps = (z_gyro * ANG_VEL_SENSITIVITY_500DPS) - offset_gz; + + // Direct assignment (Hardware Filter is doing the work) + imu_data.accel.filt_x = x_accel_ms2; + imu_data.accel.filt_y = y_accel_ms2; + imu_data.accel.filt_z = z_accel_ms2; + + imu_data.gyro.filt_x = x_gyro_dps; + imu_data.gyro.filt_y = y_gyro_dps; + imu_data.gyro.filt_z = z_gyro_dps; + + // Store raw data + imu_data.accel.x = x_accel; + imu_data.accel.y = y_accel; + imu_data.accel.z = z_accel; + + imu_data.gyro.x = x_gyro; + imu_data.gyro.y = y_gyro; + imu_data.gyro.z = z_gyro; + } +} + +LSM6DS3TR_Data_t *lsm6ds3tr_get_data(void) { return &imu_data; } \ No newline at end of file diff --git a/stm32-tests/sensor-testing/Core/Src/main.c b/stm32-tests/sensor-testing/Core/Src/main.c index 0bdf0f1..8030ed6 100644 --- a/stm32-tests/sensor-testing/Core/Src/main.c +++ b/stm32-tests/sensor-testing/Core/Src/main.c @@ -1,20 +1,20 @@ /* USER CODE BEGIN Header */ /** - ****************************************************************************** - * @file : main.c - * @brief : Main program body - ****************************************************************************** - * @attention - * - * Copyright (c) 2026 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. - * - ****************************************************************************** - */ + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 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" @@ -69,320 +69,322 @@ static void process_command(void); /* USER CODE END 0 */ /** - * @brief The application entry point. - * @retval int - */ + * @brief The application entry point. + * @retval int + */ int main(void) { - /* USER CODE BEGIN 1 */ + /* USER CODE BEGIN 1 */ - /* USER CODE END 1 */ + /* USER CODE END 1 */ - /* MCU Configuration--------------------------------------------------------*/ + /* MCU Configuration--------------------------------------------------------*/ - /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ - HAL_Init(); + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); - /* USER CODE BEGIN Init */ + /* USER CODE BEGIN Init */ - /* USER CODE END Init */ + /* USER CODE END Init */ - /* Configure the system clock */ - SystemClock_Config(); + /* Configure the system clock */ + SystemClock_Config(); - /* USER CODE BEGIN SysInit */ + /* USER CODE BEGIN SysInit */ - /* USER CODE END SysInit */ + /* USER CODE END SysInit */ - /* Initialize all configured peripherals */ - MX_GPIO_Init(); - MX_USART2_UART_Init(); - MX_I2C3_Init(); - /* USER CODE BEGIN 2 */ - lsm6ds3tr_init_driver(&hi2c3); - HAL_UART_Receive_IT(&huart2, rx_data, 1); - /* USER CODE END 2 */ + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + MX_USART2_UART_Init(); + MX_I2C3_Init(); + /* USER CODE BEGIN 2 */ + lsm6ds3tr_init_driver(&hi2c3); + HAL_UART_Receive_IT(&huart2, rx_data, 1); - /* Infinite loop */ - /* USER CODE BEGIN WHILE */ - while (1) - { - /* USER CODE END WHILE */ + // Calibration ~ 3 seconds + lsm6ds3tr_calibrate(); - /* USER CODE BEGIN 3 */ + /* USER CODE END 2 */ - // ALWAYS read values - lsm6ds3tr_read(); + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + /* USER CODE END WHILE */ - if (cmd_ready) - { - cmd_ready = 0; - process_command(); - } + /* USER CODE BEGIN 3 */ - // Add a small delay - HAL_Delay(10); - } - /* USER CODE END 3 */ + // ALWAYS read values + lsm6ds3tr_read(); + + if (cmd_ready) + { + cmd_ready = 0; + + process_command(); + } + + // Add a small delay + HAL_Delay(10); + } + /* USER CODE END 3 */ } /** - * @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 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. - */ - 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 = 16; - RCC_OscInitStruct.PLL.PLLN = 336; - RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; - RCC_OscInitStruct.PLL.PLLQ = 2; - RCC_OscInitStruct.PLL.PLLR = 2; - 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_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - - if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) - { - Error_Handler(); - } + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** 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. + */ + 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 = 16; + RCC_OscInitStruct.PLL.PLLN = 336; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; + RCC_OscInitStruct.PLL.PLLQ = 2; + RCC_OscInitStruct.PLL.PLLR = 2; + 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_DIV2; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK) + { + Error_Handler(); + } } /** - * @brief I2C3 Initialization Function - * @param None - * @retval None - */ + * @brief I2C3 Initialization Function + * @param None + * @retval None + */ static void MX_I2C3_Init(void) { - /* USER CODE BEGIN I2C3_Init 0 */ - - /* USER CODE END I2C3_Init 0 */ - - /* USER CODE BEGIN I2C3_Init 1 */ - - /* USER CODE END I2C3_Init 1 */ - hi2c3.Instance = I2C3; - hi2c3.Init.ClockSpeed = 100000; - hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2; - hi2c3.Init.OwnAddress1 = 0; - hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; - hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; - hi2c3.Init.OwnAddress2 = 0; - hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; - hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; - if (HAL_I2C_Init(&hi2c3) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN I2C3_Init 2 */ - - /* USER CODE END I2C3_Init 2 */ - + /* USER CODE BEGIN I2C3_Init 0 */ + + /* USER CODE END I2C3_Init 0 */ + + /* USER CODE BEGIN I2C3_Init 1 */ + + /* USER CODE END I2C3_Init 1 */ + hi2c3.Instance = I2C3; + hi2c3.Init.ClockSpeed = 100000; + hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2; + hi2c3.Init.OwnAddress1 = 0; + hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + hi2c3.Init.OwnAddress2 = 0; + hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; + if (HAL_I2C_Init(&hi2c3) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN I2C3_Init 2 */ + + /* USER CODE END I2C3_Init 2 */ } /** - * @brief USART2 Initialization Function - * @param None - * @retval None - */ + * @brief USART2 Initialization Function + * @param None + * @retval None + */ static void MX_USART2_UART_Init(void) { - /* USER CODE BEGIN USART2_Init 0 */ - - /* USER CODE END USART2_Init 0 */ + /* USER CODE BEGIN USART2_Init 0 */ - /* USER CODE BEGIN USART2_Init 1 */ + /* USER CODE END USART2_Init 0 */ - /* USER CODE END USART2_Init 1 */ - huart2.Instance = USART2; - huart2.Init.BaudRate = 115200; - huart2.Init.WordLength = UART_WORDLENGTH_8B; - huart2.Init.StopBits = UART_STOPBITS_1; - huart2.Init.Parity = UART_PARITY_NONE; - huart2.Init.Mode = UART_MODE_TX_RX; - huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; - huart2.Init.OverSampling = UART_OVERSAMPLING_16; - if (HAL_UART_Init(&huart2) != HAL_OK) - { - Error_Handler(); - } - /* USER CODE BEGIN USART2_Init 2 */ + /* USER CODE BEGIN USART2_Init 1 */ - /* USER CODE END USART2_Init 2 */ + /* USER CODE END USART2_Init 1 */ + huart2.Instance = USART2; + huart2.Init.BaudRate = 115200; + huart2.Init.WordLength = UART_WORDLENGTH_8B; + huart2.Init.StopBits = UART_STOPBITS_1; + huart2.Init.Parity = UART_PARITY_NONE; + huart2.Init.Mode = UART_MODE_TX_RX; + huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart2.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART2_Init 2 */ + /* USER CODE END USART2_Init 2 */ } /** - * @brief GPIO Initialization Function - * @param None - * @retval None - */ + * @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 */ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + /* USER CODE BEGIN MX_GPIO_Init_1 */ - /* USER CODE END MX_GPIO_Init_1 */ + /* USER CODE END MX_GPIO_Init_1 */ - /* GPIO Ports Clock Enable */ - __HAL_RCC_GPIOC_CLK_ENABLE(); - __HAL_RCC_GPIOH_CLK_ENABLE(); - __HAL_RCC_GPIOA_CLK_ENABLE(); - __HAL_RCC_GPIOB_CLK_ENABLE(); + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOH_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); - /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET); + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(LD2_GPIO_Port, LD2_Pin, GPIO_PIN_RESET); - /*Configure GPIO pin : B1_Pin */ - GPIO_InitStruct.Pin = B1_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : B1_Pin */ + GPIO_InitStruct.Pin = B1_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct); - /*Configure GPIO pin : LD2_Pin */ - GPIO_InitStruct.Pin = LD2_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; - HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : LD2_Pin */ + GPIO_InitStruct.Pin = LD2_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(LD2_GPIO_Port, &GPIO_InitStruct); - /* USER CODE BEGIN MX_GPIO_Init_2 */ + /* USER CODE BEGIN MX_GPIO_Init_2 */ - /* USER CODE END MX_GPIO_Init_2 */ + /* USER CODE END MX_GPIO_Init_2 */ } /* USER CODE BEGIN 4 */ void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { - UNUSED(huart); - - if (rx_data[0] == '\n' || rx_data[0] == '\r') - { - if (rx_index > 0) - { - rx_buffer[rx_index] = '\0'; - cmd_ready = 1; - rx_index = 0; - } - } - else if (rx_index < sizeof(rx_buffer) - 1) - { - rx_buffer[rx_index++] = rx_data[0]; - } - - HAL_UART_Receive_IT(&huart2, rx_data, 1); + UNUSED(huart); + + if (rx_data[0] == '\n' || rx_data[0] == '\r') + { + if (rx_index > 0) + { + rx_buffer[rx_index] = '\0'; + cmd_ready = 1; + rx_index = 0; + } + } + else if (rx_index < sizeof(rx_buffer) - 1) + { + rx_buffer[rx_index++] = rx_data[0]; + } + + HAL_UART_Receive_IT(&huart2, rx_data, 1); } static void process_command(void) { - LSM6DS3TR_Data_t *imu = lsm6ds3tr_get_data(); - uint16_t len; - - if (strcmp((char*)rx_buffer, "READ") == 0) - { - - len = sprintf((char*)tx_buffer, - "AX:%.2f AY:%.2f AZ:%.2f GX:%.2f GY:%.2f GZ:%.2f\r\n", - imu->accel.filt_x, imu->accel.filt_y, imu->accel.filt_z, - imu->gyro.filt_x, imu->gyro.filt_y, imu->gyro.filt_z); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "STATUS") == 0) - { - lsm6ds3tr_check_connection(); - if (imu->state == SENSOR_STATE_CONNECTED) - { - len = sprintf((char*)tx_buffer, "STATUS:CONNECTED\r\n"); - } - else - { - len = sprintf((char*)tx_buffer, "STATUS:LOST\r\n"); - } - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "REGISTER") == 0) - { - len = sprintf((char*)tx_buffer, "REGISTER:0x%02X\r\n", DEVICE_ADDRESS); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "CONFIG") == 0) - { - len = sprintf((char*)tx_buffer, - "GYRO_CFG:0x%02X ACCEL_CFG:0x%02X\r\n", - imu->gyro_config, imu->accel_config); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else if (strcmp((char*)rx_buffer, "POWER") == 0) - { - len = sprintf((char*)tx_buffer, "POWER_CFG:0x%02X\r\n", imu->power_config); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } - else - { - len = sprintf((char*)tx_buffer, "ERR:UNKNOWN_CMD\r\n"); - HAL_UART_Transmit(&huart2, tx_buffer, len, 100); - } + LSM6DS3TR_Data_t *imu = lsm6ds3tr_get_data(); + uint16_t len; + + if (strcmp((char *)rx_buffer, "READ") == 0) + { + + len = sprintf((char *)tx_buffer, + "AX:%.2f AY:%.2f AZ:%.2f GX:%.2f GY:%.2f GZ:%.2f\r\n", + imu->accel.filt_x, imu->accel.filt_y, imu->accel.filt_z, + imu->gyro.filt_x, imu->gyro.filt_y, imu->gyro.filt_z); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "STATUS") == 0) + { + lsm6ds3tr_check_connection(); + if (imu->state == SENSOR_STATE_CONNECTED) + { + len = sprintf((char *)tx_buffer, "STATUS:CONNECTED\r\n"); + } + else + { + len = sprintf((char *)tx_buffer, "STATUS:LOST\r\n"); + } + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "REGISTER") == 0) + { + len = sprintf((char *)tx_buffer, "REGISTER:0x%02X\r\n", DEVICE_ADDRESS); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "CONFIG") == 0) + { + len = sprintf((char *)tx_buffer, + "GYRO_CFG:0x%02X ACCEL_CFG:0x%02X\r\n", + imu->gyro_config, imu->accel_config); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else if (strcmp((char *)rx_buffer, "POWER") == 0) + { + len = sprintf((char *)tx_buffer, "POWER_CFG:0x%02X\r\n", imu->power_config); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } + else + { + len = sprintf((char *)tx_buffer, "ERR:UNKNOWN_CMD\r\n"); + HAL_UART_Transmit(&huart2, tx_buffer, len, 100); + } } /* 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 */ - /* User can add his own implementation to report the HAL error return state */ - __disable_irq(); - while (1) - { - } - /* USER CODE END Error_Handler_Debug */ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* 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 - */ + * @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 */ + /* 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 */ \ No newline at end of file