FMCZ/stabilization
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added code handling IMU and started writing stabilization code

Browse Source
This commit is contained in:
Angoosh Leviocki
2025-08-09 00:54:26 +02:00
parent f24948bc09
commit 5d86bcfe26
220 changed files with 155202 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
Core/Inc/MPU6000.h Normal file
View File

@@ -0,0 +1,120 @@
/*
* MPU6000.h
*
* Created on: Nov 24, 2021
* Author: angoosh
*/
#ifndef INC_MPU6000_H_
#define INC_MPU6000_H_
#include "stm32g0xx_hal.h"
#define MPU6000_ADDRESS 0x68//0x68
#define ACCEL_RANGE 0 //2048 LSB/g
#define GYRO_RANGE 0 //32.8 LSB/deg/s
#define PERFORM_SELFTEST 0
//register description https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf
typedef enum {
GYRO_CONFIG = 0x1B,// register 27
ACCEL_CONFIG = 0x1C,// register 28
FIFO_EN = 0x23,
ACCEL_XOUT_H = 0x3B,
ACCEL_XOUT_L = 0x3C,
ACCEL_YOUT_H = 0x3D,
ACCEL_YOUT_L = 0x3E,
ACCEL_ZOUT_H = 0x3F,
ACCEL_ZOUT_L = 0x40,//sampl rate = reg 25 ; scale in reg 28
TEMP_OUT_H = 0x41,
TEMP_OUT_L = 0x42,
GYRO_XOUT_H = 0x43,
GYRO_XOUT_L = 0x44,
GYRO_YOUT_H = 0x45,
GYRO_YOUT_L = 0x46,
GYRO_ZOUT_H = 0x47,
GYRO_ZOUT_L = 0x48,// register 27 full scale setup
SENSOR_RESET = 0x68,
PWR_MGMT_1 = 0x6B,
PWR_MGMT_2 = 0x6C,
SMPLRT_DIV = 0x19,//register 25
WHO_AM_I = 0x75
} MPU6000_REG;
typedef enum {
ACCEL_16384_LSB_g = 0,
ACCEL_8192_LSB_g = 1,
ACCEL_4096_LSB_g = 2,
ACCEL_2048_LSB_g = 3,
GYRO_131_LSB_deg_s = 0,
GYRO_65_5_LSB_deg_s = 1,
GYRO_32_8_LSB_deg_s = 2,
GYRO_16_4_LSB_deg_s = 3,
} MPU6000_SCALE;
typedef struct{
float g_x;
float g_y;
float g_z;
float a_x;
float a_y;
float a_z;
int16_t cal_g_x;
int16_t cal_g_y;
int16_t cal_g_z;
int16_t cal_a_x;
int16_t cal_a_y;
int16_t cal_a_z;
float pitch;
float roll;
float yaw;
uint8_t gyro_data[6];
uint8_t accel_data[6];
uint8_t all_data[14];
float gyro_LSB;
float accel_LSB;
uint32_t pedo_steps;
float float_pedo_steps;
float pedo_threshold;
float pedo_work_vector[100];
float pedo_vector[100];
float pedo_a_x[100];
float pedo_a_y[100];
float pedo_a_z[100];
float pedo_avg_a_x;
float pedo_avg_a_y;
float pedo_avg_a_z;
uint8_t pedo_index;
uint8_t pedo_flag;
float pedo_vector_max;
float pedo_vector_avg;
uint32_t pedo_vector_samples;
uint32_t pedo_last_step_timestamp;
uint8_t rx_expected_flag;
I2C_HandleTypeDef *peripheral;
uint32_t timeout;
uint8_t dataReady;
} MPU6000_Typedef;
extern MPU6000_Typedef IMU;
void MPU6000_readGyro();
void MPU6000_readAccel();
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout);
uint8_t MPU6000_readRegister(uint8_t reg);
void MPU6000_Calibrate();
void MPU6000_Gyro();
void MPU6000_Raw_Data_Convert();
void MPU6000_readAll();
void MPU6000_Pedometer_Init();
void MPU6000_Pedometer();
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c);
void IMU_Check_State();
#endif /* INC_MPU6000_H_ */

128
Core/Inc/madgwick.h Normal file
View File

@@ -0,0 +1,128 @@
// MIT License
// Copyright (c) 2020 phonght32, 2025 edited by angoosh for general use
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
#ifndef _IMU_MADGWICK_H_
#define _IMU_MADGWICK_H_
#ifdef __cplusplus
extern "C" {
#endif
#include "stdint.h"
#include "math.h"
#define MADGWICK_BETA 0.2f
#define MADGWICK_SAMPLE_RATE 250.0f
#define DEG2RAD 3.14f/180.0f
typedef struct madgwick *madgwick_handle_t;
typedef struct {
float q0;
float q1;
float q2;
float q3;
} madgwick_quat_data_t;
typedef struct {
float beta;
float sample_freq;
} madgwick_cfg_t;
/*
* @brief Configure Madgwick AHRS parameters.
* @param config Struct pointer.
* @return
* - Madgwick handle structure: Success.
* - 0: Fail.
*/
madgwick_handle_t madgwick_init(madgwick_cfg_t *config);
/*
* @brief Set beta value.
* @param handle Handle structure.
* @param beta Beta.
* @return
* - STM_OK: Success.
* - Others: Fail.
*/
uint8_t madgwick_set_beta(madgwick_handle_t handle, float beta);
/*
* @brief Set sample frequency value.
* @param handle Handle structure.
* @param sample_freq Sample frequency.
* @return
* - STM_OK: Success.
* - Others: Fail.
*/
uint8_t madgwick_set_sample_frequency(madgwick_handle_t handle, float sample_freq);
/*
* @brief Get quaternion.
* @param handle Handle structure.
* @param quat_data Quaternion.
* @return
* - STM_OK: Success.
* - Others: Fail.
*/
uint8_t madgwick_get_quaternion(madgwick_handle_t handle, madgwick_quat_data_t *quat_data);
/*
* @brief Update Madgwick AHRS quaternion with 6 motions.
* @param handle Handle structure.
* @param gx Gyroscope along x axis.
* @param gy Gyroscope along y axis.
* @param gz Gyroscope along z axis.
* @param ax Accelerometer along x axis.
* @param ay Accelerometer along y axis.
* @param az Accelerometer along z axis.
* @return
* - STM_OK: Success.
* - Others: Fail.
*/
uint8_t madgwick_update_6dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az);
/*
* @brief Update Madgwick AHRS quaternion with 9 motions.
* @param handle Handle structure.
* @param gx Gyroscope along x axis.
* @param gy Gyroscope along y axis.
* @param gz Gyroscope along z axis.
* @param ax Accelerometer along x axis.
* @param ay Accelerometer along y axis.
* @param az Accelerometer along z axis.
* @param mx Magnetometer along x axis.
* @param my Magnetometer along y axis.
* @param mz Magnetometer along z axis.
* @return
* - STM_OK: Success.
* - Others: Fail.
*/
uint8_t madgwick_update_9dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
#ifdef __cplusplus
}
#endif
#endif /* _IMU_MADGWICK_H_ */

75
Core/Inc/main.h Normal file
View File

@@ -0,0 +1,75 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.h
* @brief : Header for main.c file.
* This file contains the common defines of the application.
******************************************************************************
* @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 */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32g0xx_hal.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "madgwick.h"
#include "MPU6000.h"
#include "stabilize.h"
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
/* Exported functions prototypes ---------------------------------------------*/
void Error_Handler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
/* Private defines -----------------------------------------------------------*/
#define LED_Pin GPIO_PIN_15
#define LED_GPIO_Port GPIOC
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
#ifdef __cplusplus
}
#endif
#endif /* __MAIN_H */

22
Core/Inc/stabilize.h Normal file
View File

@@ -0,0 +1,22 @@
/*
* stabilize.h
*
* Created on: Aug 9, 2025
* Author: angoosh
*/
#ifndef INC_STABILIZE_H_
#define INC_STABILIZE_H_
#include "main.h"
#include "MPU6000.h"
typedef struct{
float roll_gain;
float pitch_gain;
float yaw_gain;
} Stabilize_Typedef;
extern Stabilize_Typedef STAB;
#endif /* INC_STABILIZE_H_ */

351
Core/Inc/stm32g0xx_hal_conf.h Normal file
View File

@@ -0,0 +1,351 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g0xx_hal_conf.h
* @author MCD Application Team
* @brief HAL configuration file.
******************************************************************************
* @attention
*
* Copyright (c) 2018 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 */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef STM32G0xx_HAL_CONF_H
#define STM32G0xx_HAL_CONF_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* ########################## Module Selection ############################## */
/**
* @brief This is the list of modules to be used in the HAL driver
*/
#define HAL_MODULE_ENABLED
/* #define HAL_ADC_MODULE_ENABLED */
/* #define HAL_CEC_MODULE_ENABLED */
/* #define HAL_COMP_MODULE_ENABLED */
/* #define HAL_CRC_MODULE_ENABLED */
/* #define HAL_CRYP_MODULE_ENABLED */
/* #define HAL_DAC_MODULE_ENABLED */
/* #define HAL_EXTI_MODULE_ENABLED */
/* #define HAL_FDCAN_MODULE_ENABLED */
/* #define HAL_HCD_MODULE_ENABLED */
#define HAL_I2C_MODULE_ENABLED
/* #define HAL_I2S_MODULE_ENABLED */
/* #define HAL_IWDG_MODULE_ENABLED */
/* #define HAL_IRDA_MODULE_ENABLED */
/* #define HAL_LPTIM_MODULE_ENABLED */
/* #define HAL_PCD_MODULE_ENABLED */
/* #define HAL_RNG_MODULE_ENABLED */
/* #define HAL_RTC_MODULE_ENABLED */
/* #define HAL_SMARTCARD_MODULE_ENABLED */
/* #define HAL_SMBUS_MODULE_ENABLED */
/* #define HAL_SPI_MODULE_ENABLED */
#define HAL_TIM_MODULE_ENABLED
/* #define HAL_UART_MODULE_ENABLED */
/* #define HAL_USART_MODULE_ENABLED */
/* #define HAL_WWDG_MODULE_ENABLED */
#define HAL_GPIO_MODULE_ENABLED
#define HAL_EXTI_MODULE_ENABLED
#define HAL_DMA_MODULE_ENABLED
#define HAL_RCC_MODULE_ENABLED
#define HAL_FLASH_MODULE_ENABLED
#define HAL_PWR_MODULE_ENABLED
#define HAL_CORTEX_MODULE_ENABLED
/* ########################## Register Callbacks selection ############################## */
/**
* @brief This is the list of modules where register callback can be used
*/
#define USE_HAL_ADC_REGISTER_CALLBACKS 0u
#define USE_HAL_CEC_REGISTER_CALLBACKS 0u
#define USE_HAL_COMP_REGISTER_CALLBACKS 0u
#define USE_HAL_CRYP_REGISTER_CALLBACKS 0u
#define USE_HAL_DAC_REGISTER_CALLBACKS 0u
#define USE_HAL_FDCAN_REGISTER_CALLBACKS 0u
#define USE_HAL_HCD_REGISTER_CALLBACKS 0u
#define USE_HAL_I2C_REGISTER_CALLBACKS 0u
#define USE_HAL_I2S_REGISTER_CALLBACKS 0u
#define USE_HAL_IRDA_REGISTER_CALLBACKS 0u
#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0u
#define USE_HAL_PCD_REGISTER_CALLBACKS 0u
#define USE_HAL_RNG_REGISTER_CALLBACKS 0u
#define USE_HAL_RTC_REGISTER_CALLBACKS 0u
#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0u
#define USE_HAL_SPI_REGISTER_CALLBACKS 0u
#define USE_HAL_TIM_REGISTER_CALLBACKS 0u
#define USE_HAL_UART_REGISTER_CALLBACKS 0u
#define USE_HAL_USART_REGISTER_CALLBACKS 0u
#define USE_HAL_WWDG_REGISTER_CALLBACKS 0u
/* ########################## Oscillator Values adaptation ####################*/
/**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL).
*/
#if !defined (HSE_VALUE)
#define HSE_VALUE (8000000UL) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSE_STARTUP_TIMEOUT)
#define HSE_STARTUP_TIMEOUT (100UL) /*!< Time out for HSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */
/**
* @brief Internal High Speed oscillator (HSI) value.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSI is used as system clock source, directly or through the PLL).
*/
#if !defined (HSI_VALUE)
#define HSI_VALUE (16000000UL) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
#if defined(STM32G0C1xx) || defined(STM32G0B1xx) || defined(STM32G0B0xx)
/**
* @brief Internal High Speed oscillator (HSI48) value for USB FS, SDMMC and RNG.
* This internal oscillator is mainly dedicated to provide a high precision clock to
* the USB peripheral by means of a special Clock Recovery System (CRS) circuitry.
* When the CRS is not used, the HSI48 RC oscillator runs on it default frequency
* which is subject to manufacturing process variations.
*/
#if !defined (HSI48_VALUE)
#define HSI48_VALUE 48000000U /*!< Value of the Internal High Speed oscillator for USB FS/SDMMC/RNG in Hz.
The real value my vary depending on manufacturing process variations.*/
#endif /* HSI48_VALUE */
#endif
/**
* @brief Internal Low Speed oscillator (LSI) value.
*/
#if !defined (LSI_VALUE)
#define LSI_VALUE (32000UL) /*!< LSI Typical Value in Hz*/
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
The real value may vary depending on the variations
in voltage and temperature.*/
/**
* @brief External Low Speed oscillator (LSE) value.
* This value is used by the UART, RTC HAL module to compute the system frequency
*/
#if !defined (LSE_VALUE)
#define LSE_VALUE (32768UL) /*!< Value of the External oscillator in Hz*/
#endif /* LSE_VALUE */
#if !defined (LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT (5000UL) /*!< Time out for LSE start up, in ms */
#endif /* LSE_STARTUP_TIMEOUT */
/**
* @brief External clock source for I2S1 peripheral
* This value is used by the RCC HAL module to compute the I2S1 clock source
* frequency.
*/
#if !defined (EXTERNAL_I2S1_CLOCK_VALUE)
#define EXTERNAL_I2S1_CLOCK_VALUE (12288000UL) /*!< Value of the I2S1 External clock source in Hz*/
#endif /* EXTERNAL_I2S1_CLOCK_VALUE */
#if defined(STM32G0C1xx) || defined(STM32G0B1xx) || defined(STM32G0B0xx)
/**
* @brief External clock source for I2S2 peripheral
* This value is used by the RCC HAL module to compute the I2S2 clock source
* frequency.
*/
#if !defined (EXTERNAL_I2S2_CLOCK_VALUE)
#define EXTERNAL_I2S2_CLOCK_VALUE 48000U /*!< Value of the I2S2 External clock source in Hz*/
#endif /* EXTERNAL_I2S2_CLOCK_VALUE */
#endif
/* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */
/* ########################### System Configuration ######################### */
/**
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE (3300UL) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY 3U /*!< tick interrupt priority */
#define USE_RTOS 0U
#define PREFETCH_ENABLE 1U
#define INSTRUCTION_CACHE_ENABLE 1U
/* ################## SPI peripheral configuration ########################## */
/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver
* Activated: CRC code is present inside driver
* Deactivated: CRC code cleaned from driver
*/
#define USE_SPI_CRC 0U
/* ################## CRYP peripheral configuration ########################## */
#define USE_HAL_CRYP_SUSPEND_RESUME 1U
/* ########################## Assert Selection ############################## */
/**
* @brief Uncomment the line below to expanse the "assert_param" macro in the
* HAL drivers code
*/
/* #define USE_FULL_ASSERT 1U */
/* Includes ------------------------------------------------------------------*/
/**
* @brief Include modules header file
*/
#ifdef HAL_RCC_MODULE_ENABLED
#include "stm32g0xx_hal_rcc.h"
#endif /* HAL_RCC_MODULE_ENABLED */
#ifdef HAL_GPIO_MODULE_ENABLED
#include "stm32g0xx_hal_gpio.h"
#endif /* HAL_GPIO_MODULE_ENABLED */
#ifdef HAL_DMA_MODULE_ENABLED
#include "stm32g0xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_CORTEX_MODULE_ENABLED
#include "stm32g0xx_hal_cortex.h"
#endif /* HAL_CORTEX_MODULE_ENABLED */
#ifdef HAL_ADC_MODULE_ENABLED
#include "stm32g0xx_hal_adc.h"
#include "stm32g0xx_hal_adc_ex.h"
#endif /* HAL_ADC_MODULE_ENABLED */
#ifdef HAL_CEC_MODULE_ENABLED
#include "stm32g0xx_hal_cec.h"
#endif /* HAL_CEC_MODULE_ENABLED */
#ifdef HAL_COMP_MODULE_ENABLED
#include "stm32g0xx_hal_comp.h"
#endif /* HAL_COMP_MODULE_ENABLED */
#ifdef HAL_CRC_MODULE_ENABLED
#include "stm32g0xx_hal_crc.h"
#endif /* HAL_CRC_MODULE_ENABLED */
#ifdef HAL_CRYP_MODULE_ENABLED
#include "stm32g0xx_hal_cryp.h"
#endif /* HAL_CRYP_MODULE_ENABLED */
#ifdef HAL_DAC_MODULE_ENABLED
#include "stm32g0xx_hal_dac.h"
#endif /* HAL_DAC_MODULE_ENABLED */
#ifdef HAL_EXTI_MODULE_ENABLED
#include "stm32g0xx_hal_exti.h"
#endif /* HAL_EXTI_MODULE_ENABLED */
#ifdef HAL_FLASH_MODULE_ENABLED
#include "stm32g0xx_hal_flash.h"
#endif /* HAL_FLASH_MODULE_ENABLED */
#ifdef HAL_FDCAN_MODULE_ENABLED
#include "stm32g0xx_hal_fdcan.h"
#endif /* HAL_FDCAN_MODULE_ENABLED */
#ifdef HAL_HCD_MODULE_ENABLED
#include "stm32g0xx_hal_hcd.h"
#endif /* HAL_HCD_MODULE_ENABLED */
#ifdef HAL_I2C_MODULE_ENABLED
#include "stm32g0xx_hal_i2c.h"
#endif /* HAL_I2C_MODULE_ENABLED */
#ifdef HAL_I2S_MODULE_ENABLED
#include "stm32g0xx_hal_i2s.h"
#endif /* HAL_I2S_MODULE_ENABLED */
#ifdef HAL_IRDA_MODULE_ENABLED
#include "stm32g0xx_hal_irda.h"
#endif /* HAL_IRDA_MODULE_ENABLED */
#ifdef HAL_IWDG_MODULE_ENABLED
#include "stm32g0xx_hal_iwdg.h"
#endif /* HAL_IWDG_MODULE_ENABLED */
#ifdef HAL_LPTIM_MODULE_ENABLED
#include "stm32g0xx_hal_lptim.h"
#endif /* HAL_LPTIM_MODULE_ENABLED */
#ifdef HAL_PCD_MODULE_ENABLED
#include "stm32g0xx_hal_pcd.h"
#endif /* HAL_PCD_MODULE_ENABLED */
#ifdef HAL_PWR_MODULE_ENABLED
#include "stm32g0xx_hal_pwr.h"
#endif /* HAL_PWR_MODULE_ENABLED */
#ifdef HAL_RNG_MODULE_ENABLED
#include "stm32g0xx_hal_rng.h"
#endif /* HAL_RNG_MODULE_ENABLED */
#ifdef HAL_RTC_MODULE_ENABLED
#include "stm32g0xx_hal_rtc.h"
#endif /* HAL_RTC_MODULE_ENABLED */
#ifdef HAL_SMARTCARD_MODULE_ENABLED
#include "stm32g0xx_hal_smartcard.h"
#endif /* HAL_SMARTCARD_MODULE_ENABLED */
#ifdef HAL_SMBUS_MODULE_ENABLED
#include "stm32g0xx_hal_smbus.h"
#endif /* HAL_SMBUS_MODULE_ENABLED */
#ifdef HAL_SPI_MODULE_ENABLED
#include "stm32g0xx_hal_spi.h"
#endif /* HAL_SPI_MODULE_ENABLED */
#ifdef HAL_TIM_MODULE_ENABLED
#include "stm32g0xx_hal_tim.h"
#endif /* HAL_TIM_MODULE_ENABLED */
#ifdef HAL_UART_MODULE_ENABLED
#include "stm32g0xx_hal_uart.h"
#endif /* HAL_UART_MODULE_ENABLED */
#ifdef HAL_USART_MODULE_ENABLED
#include "stm32g0xx_hal_usart.h"
#endif /* HAL_USART_MODULE_ENABLED */
#ifdef HAL_WWDG_MODULE_ENABLED
#include "stm32g0xx_hal_wwdg.h"
#endif /* HAL_WWDG_MODULE_ENABLED */
/* Exported macro ------------------------------------------------------------*/
#ifdef USE_FULL_ASSERT
/**
* @brief The assert_param macro is used for functions parameters check.
* @param expr If expr is false, it calls assert_failed function
* which reports the name of the source file and the source
* line number of the call that failed.
* If expr is true, it returns no value.
* @retval None
*/
#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__))
/* Exported functions ------------------------------------------------------- */
void assert_failed(uint8_t *file, uint32_t line);
#else
#define assert_param(expr) ((void)0U)
#endif /* USE_FULL_ASSERT */
#ifdef __cplusplus
}
#endif
#endif /* STM32G0xx_HAL_CONF_H */

63
Core/Inc/stm32g0xx_it.h Normal file
View File

@@ -0,0 +1,63 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g0xx_it.h
* @brief This file contains the headers of the interrupt handlers.
******************************************************************************
* @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 */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32G0xx_IT_H
#define __STM32G0xx_IT_H
#ifdef __cplusplus
extern "C" {
#endif
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
/* Exported functions prototypes ---------------------------------------------*/
void NMI_Handler(void);
void HardFault_Handler(void);
void SVC_Handler(void);
void PendSV_Handler(void);
void SysTick_Handler(void);
void I2C1_IRQHandler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
#ifdef __cplusplus
}
#endif
#endif /* __STM32G0xx_IT_H */

305
Core/Src/MPU6000.c Normal file
View File

@@ -0,0 +1,305 @@
/*
* MPU6000.c
*
* Created on: Nov 24, 2021
* Author: angoosh
*/
#include "MPU6000.h"
#include "main.h"
#include <math.h>
MPU6000_Typedef IMU;
uint32_t sysclock = 0;
void MPU6000_readGyro();
void MPU6000_readAccel();
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout);
uint8_t MPU6000_readRegister(uint8_t reg);
void MPU6000_Calibrate();
void MPU6000_Gyro();
void MPU6000_Raw_Data_Convert();
void MPU6000_readAll();
void MPU6000_Pedometer_Init();
void MPU6000_Pedometer();
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c);
void IMU_Check_State();
madgwick_handle_t madgwick_handle;
madgwick_quat_data_t quat_data;
/**
* @brief Converts raw sensor data to usable format
* @retval none
*/
void MPU6000_Raw_Data_Convert(){
for(int i = 0; i < 6; i++){
IMU.accel_data[i] = IMU.all_data[i];
IMU.gyro_data[i] = IMU.all_data[i+8];
}
IMU.g_x = (float)((int16_t)(IMU.gyro_data[0] * 256 + IMU.gyro_data[1]) - IMU.cal_g_x) / IMU.gyro_LSB;
IMU.g_y = (float)((int16_t)(IMU.gyro_data[2] * 256 + IMU.gyro_data[3]) - IMU.cal_g_y) / IMU.gyro_LSB;
IMU.g_z = (float)((int16_t)(IMU.gyro_data[4] * 256 + IMU.gyro_data[5]) - IMU.cal_g_z) / IMU.gyro_LSB;
IMU.a_x = (float)((int16_t)(IMU.accel_data[0] * 256 + IMU.accel_data[1]) - IMU.cal_a_x) / IMU.accel_LSB;
IMU.a_y = (float)((int16_t)(IMU.accel_data[2] * 256 + IMU.accel_data[3]) - IMU.cal_a_y) / IMU.accel_LSB;
IMU.a_z = (float)((int16_t)(IMU.accel_data[4] * 256 + IMU.accel_data[5]) - IMU.cal_a_z) / IMU.accel_LSB;
}
/**
* @brief Read all data on the sensor
* @retval none
*/
void MPU6000_readAll(){
HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_XOUT_H,1,IMU.all_data,14);
}
/**
* @brief Read only gyroscope
* @retval none
*/
void MPU6000_readGyro(){
switch(GYRO_RANGE){
case 0:
IMU.gyro_LSB = 131;
case 1:
IMU.gyro_LSB = 65.5;
case 2:
IMU.gyro_LSB = 32.8;
case 3:
IMU.gyro_LSB = 16.4;
default:
IMU.gyro_LSB = 16.4;
}
HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),GYRO_XOUT_H,1,IMU.gyro_data,6);
}
/**
* @brief Read only accelerometer
* @retval none
*/
void MPU6000_readAccel(){
switch(GYRO_RANGE){
case 0:
IMU.accel_LSB = 16384;
case 1:
IMU.accel_LSB = 8192;
case 2:
IMU.accel_LSB = 4096;
case 3:
IMU.accel_LSB = 2048;
default:
IMU.accel_LSB = 2048;
}
HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_XOUT_H,1,IMU.accel_data,6);
}
/**
* @brief Converts accelerometer data to roll pitch and yaw
* @retval none
*/
void MPU6000_Gyro(){
IMU.roll = atan2(IMU.a_y, IMU.a_z) * 180.0 / M_PI;
IMU.pitch = atan2(IMU.a_x, IMU.a_z) * 180.0 / M_PI;
IMU.yaw = atan2(IMU.a_x, IMU.a_y) * 180.0 / M_PI;
}
/**
* @brief Initialization function of MPU6000 library
* @param i2c peripheral to be used
* @param i2c timeout
* @retval none
*/
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout){
uint8_t data;
IMU.accel_LSB = 16384;
IMU.gyro_LSB = 131;
IMU.peripheral = hi2c;
IMU.timeout = timeout;
IMU.dataReady = 0;
madgwick_cfg_t madgwick_cfg;
madgwick_cfg.beta = MADGWICK_BETA;
madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE;
madgwick_handle = madgwick_init(&madgwick_cfg);
if(HAL_I2C_IsDeviceReady(hi2c, (uint16_t)(MPU6000_ADDRESS << 1), 3, IMU.timeout) != 0){
return 1;
}
data = 0x00;
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
HAL_Delay(200);
data = 0x07;
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),SMPLRT_DIV,1,&data,1,IMU.timeout);
HAL_Delay(50);
data = 0x00;
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_CONFIG,1,&data,1,IMU.timeout);
HAL_Delay(50);
data = 0x00;
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),GYRO_CONFIG,1,&data,1,IMU.timeout);
HAL_Delay(50);
// data = 0;
// HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
// HAL_Delay(50);
// data = 0x08;//0x28 pro lowpower rezim s frekvenci 1.25Hz
// HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
// HAL_Delay(50);
data = 0x00;
HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_2,1,&data,1,IMU.timeout);
HAL_Delay(50);
MPU6000_readAll();
return 0;
}
/**
* @brief Read certain register of sensor
* @param register address
* @retval value of register
*/
uint8_t MPU6000_readRegister(uint8_t reg){
uint8_t reg_data[1];
HAL_I2C_Mem_Read(IMU.peripheral,(MPU6000_ADDRESS << 1),reg,1,reg_data,1,IMU.timeout);
HAL_Delay(50);
return reg_data[0];
}
/**
* @brief Calibrate the sensor
* @retval none
*/
void MPU6000_Calibrate(){
uint8_t data[1] = {GYRO_XOUT_H};
uint8_t data_out[6];
HAL_I2C_Master_Transmit(IMU.peripheral, (MPU6000_ADDRESS << 1), data, 1, IMU.timeout);
HAL_I2C_Master_Receive(IMU.peripheral, (MPU6000_ADDRESS << 1), data_out, 6, IMU.timeout);
IMU.cal_g_x = data_out[0] * 256 + data_out[1];
IMU.cal_g_y = data_out[2] * 256 + data_out[3];
IMU.cal_g_z = data_out[4] * 256 + data_out[5];
data[0] = ACCEL_XOUT_H;
HAL_I2C_Master_Transmit(IMU.peripheral, (MPU6000_ADDRESS << 1), data, 1, IMU.timeout);
HAL_I2C_Master_Receive(IMU.peripheral, (MPU6000_ADDRESS << 1), data_out, 6, IMU.timeout);
IMU.cal_a_x = data_out[0] * 256 + data_out[1];
IMU.cal_a_y = data_out[2] * 256 + data_out[3];
IMU.cal_a_z = data_out[4] * 256 + data_out[5];
}
/**
* @brief Initialize pedometer
* @retval none
*/
void MPU6000_Pedometer_Init(){
IMU.pedo_index = 1;
IMU.pedo_steps = 0;
IMU.pedo_vector[0] = 0;
IMU.pedo_flag = 0;
IMU.pedo_threshold = 2000;
IMU.pedo_vector_max = 0;
IMU.pedo_vector_avg = 0;
IMU.pedo_vector_samples = 0;
sysclock = HAL_RCC_GetSysClockFreq();
}
/**
* @brief Pedometer calculations
* @retval none
*/
void MPU6000_Pedometer(){
IMU.pedo_a_x[IMU.pedo_index] = IMU.a_x * IMU.accel_LSB;
IMU.pedo_a_y[IMU.pedo_index] = IMU.a_y * IMU.accel_LSB;
IMU.pedo_a_z[IMU.pedo_index] = IMU.a_z * IMU.accel_LSB;
IMU.pedo_work_vector[IMU.pedo_index] = sqrt(((IMU.pedo_a_x[IMU.pedo_index] - IMU.pedo_avg_a_x) * (IMU.pedo_a_x[IMU.pedo_index] - IMU.pedo_avg_a_x)) + ((IMU.pedo_a_y[IMU.pedo_index] - IMU.pedo_avg_a_y) * (IMU.pedo_a_y[IMU.pedo_index] - IMU.pedo_avg_a_y)) + ((IMU.pedo_a_z[IMU.pedo_index] - IMU.pedo_avg_a_z) * (IMU.pedo_a_z[IMU.pedo_index] - IMU.pedo_avg_a_z)));
IMU.pedo_vector[0] = (IMU.pedo_work_vector[IMU.pedo_index] + IMU.pedo_work_vector[IMU.pedo_index - 1]) / 2 ;
if(IMU.pedo_vector[0] > IMU.pedo_threshold && IMU.pedo_flag == 0){
IMU.pedo_steps = IMU.pedo_steps + 1;
IMU.pedo_last_step_timestamp = HAL_GetTick();
IMU.pedo_flag = 1;
}
else if (IMU.pedo_vector[0] > IMU.pedo_threshold && IMU.pedo_flag == 1){
// Don't Count
}
if (IMU.pedo_vector[0] < IMU.pedo_threshold && IMU.pedo_flag == 1){
IMU.pedo_flag = 0;
}
if (IMU.pedo_steps < 0) {
IMU.pedo_steps = 0;
}
IMU.pedo_index++;
if(IMU.pedo_work_vector[IMU.pedo_index] > IMU.pedo_vector_max){
IMU.pedo_vector_max = IMU.pedo_work_vector[IMU.pedo_index];
}
IMU.pedo_vector_avg = IMU.pedo_vector_avg + (IMU.pedo_vector[0] - IMU.pedo_vector_avg) / ((float)IMU.pedo_vector_samples + 1);
IMU.pedo_vector_samples ++;
if(IMU.pedo_index >= 100){
IMU.pedo_index = 1;
IMU.pedo_vector[0] = 0;
float sum[3] = {0};
for(int i = 0; i < 100; i++){
sum[0] = IMU.pedo_a_x[i] + sum[0];
sum[1] = IMU.pedo_a_y[i] + sum[1];
sum[2] = IMU.pedo_a_z[i] + sum[2];
}
IMU.pedo_avg_a_x = sum[0] / 100.0;
IMU.pedo_avg_a_y = sum[1] / 100.0;
IMU.pedo_avg_a_z = sum[2] / 100.0;
}
}
/**
* @brief Function to be put into i2c callback function
* @retval none
*/
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c){
if(hi2c -> Instance == IMU.peripheral->Instance){
IMU.peripheral->Instance->CR1 &= ~(1 << 1);
IMU.dataReady = 1;
// MPU6000_Raw_Data_Convert();
// madgwick_update_6dof(madgwick_handle, IMU.g_x * DEG2RAD, IMU.g_y * DEG2RAD, IMU.g_z * DEG2RAD, IMU.a_x, IMU.a_y, IMU.a_z);
// madgwick_get_quaternion(madgwick_handle, &quat_data);
// IMU.roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
// IMU.pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
// IMU.yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
//
// IMU.dataReady = 0;
// MPU6000_readAll();
}
}
void IMU_Check_State(){
if(IMU.dataReady){
MPU6000_Raw_Data_Convert();
madgwick_update_6dof(madgwick_handle, IMU.g_x * DEG2RAD, IMU.g_y * DEG2RAD, IMU.g_z * DEG2RAD, IMU.a_x, IMU.a_y, IMU.a_z);
madgwick_get_quaternion(madgwick_handle, &quat_data);
IMU.roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
IMU.pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
IMU.yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
IMU.dataReady = 0;
MPU6000_readAll();
}
}

306
Core/Src/madgwick.c Normal file
View File

@@ -0,0 +1,306 @@
#include "madgwick.h"
#include <stddef.h>
#include <stdlib.h>
#define MADGWICK_INIT_ERR_STR "Madgwick AHRS init error"
#define MADGWICK_SET_BETA_ERR_STR "Madgwick set beta error"
#define MADGWICK_SET_SAMP_FREQ_ERR_STR "Madgwick set sample frequency error"
#define MADGWICK_GET_QUAT_ERR_STR "Madgwick get quaternion error"
#define MADGWICK_UPDATE_6DOF_ERR_STR "Madgwick update 6DOF error"
#define MADGWICK_UPDATE_9DOF_ERR_STR "Madgwick update 9DOF error"
static const char* MADGWICK_TAG = "MADGWICK AHRS";
#define MADGWICK_CHECK(a, str, ret) if(!(a)) { \
return (ret); \
}
#define STM_ERR_INVALID_ARG 1
typedef struct madgwick {
float beta;
float sample_freq;
float q0;
float q1;
float q2;
float q3;
uint8_t lock;
} madgwick_t;
static float invSqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i >> 1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
madgwick_handle_t madgwick_init(madgwick_cfg_t *config)
{
/* Check input conditions */
MADGWICK_CHECK(config, MADGWICK_INIT_ERR_STR, NULL);
/* Allocate memory for handle structure */
madgwick_handle_t handle = calloc(1, sizeof(madgwick_t));
MADGWICK_CHECK(handle, MADGWICK_INIT_ERR_STR, NULL);
/* Update handle structure */
handle->beta = config->beta;
handle->sample_freq = config->sample_freq;
handle->q0 = 1.0f;
handle->q1 = 0.0f;
handle->q2 = 0.0f;
handle->q3 = 0.0f;
handle->lock = 0;
return handle;
}
uint8_t madgwick_set_beta(madgwick_handle_t handle, float beta)
{
/* Check input conditions */
MADGWICK_CHECK(handle, MADGWICK_SET_BETA_ERR_STR, STM_ERR_INVALID_ARG);
handle->lock = 1;
handle->beta = beta;
handle->lock = 0;
return 0;
}
uint8_t madgwick_set_sample_frequency(madgwick_handle_t handle, float sample_freq)
{
/* Check input conditions */
MADGWICK_CHECK(handle, MADGWICK_SET_SAMP_FREQ_ERR_STR, STM_ERR_INVALID_ARG);
handle->lock = 1;
handle->sample_freq = sample_freq;
handle->lock = 0;
return 0;
}
uint8_t madgwick_get_quaternion(madgwick_handle_t handle, madgwick_quat_data_t *quat_data)
{
/* Check input conditions */
MADGWICK_CHECK(handle, MADGWICK_GET_QUAT_ERR_STR, STM_ERR_INVALID_ARG);
handle->lock = 1;
quat_data->q0 = handle->q0;
quat_data->q1 = handle->q1;
quat_data->q2 = handle->q2;
quat_data->q3 = handle->q3;
handle->lock = 0;
return 0;
}
uint8_t madgwick_update_6dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az)
{
/* Check input conditions */
MADGWICK_CHECK(handle, MADGWICK_UPDATE_6DOF_ERR_STR, STM_ERR_INVALID_ARG);
handle->lock = 1;
float q0 = handle->q0;
float q1 = handle->q1;
float q2 = handle->q2;
float q3 = handle->q3;
float beta = handle->beta;
float sampleFreq = handle->sample_freq;
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 , _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
handle->q0 = q0;
handle->q1 = q1;
handle->q2 = q2;
handle->q3 = q3;
handle->lock = 0;
return 0;
}
uint8_t madgwick_update_9dof(madgwick_handle_t handle, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
/* Check input conditions */
MADGWICK_CHECK(handle, MADGWICK_UPDATE_9DOF_ERR_STR, STM_ERR_INVALID_ARG);
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
madgwick_update_6dof(handle, gx, gy, gz, ax, ay, az);
return 0;
}
handle->lock = 1;
float q0 = handle->q0;
float q1 = handle->q1;
float q2 = handle->q2;
float q3 = handle->q3;
float beta = handle->beta;
float sampleFreq = handle->sample_freq;
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
handle->q0 = q0;
handle->q1 = q1;
handle->q2 = q2;
handle->q3 = q3;
handle->lock = 0;
return 0;
}

34
Core/Src/madwick_example.c Normal file
View File

@@ -0,0 +1,34 @@
#include "madgwick.h"
#include "stdint.h"
#include "stdio.h"
#define MADGWICK_BETA 0.1f
#define MADGWICK_SAMPLE_RATE 100.0f
#define DEG2RAD 3.14f/180.0f
madgwick_handle_t madgwick_handle;
madgwick_quat_data_t quat_data;
int main(){
madgwick_cfg_t madgwick_cfg;
madgwick_cfg.beta = MADGWICK_BETA;
madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE;
madgwick_handle = madgwick_init(&madgwick_cfg);
madgwick_update_6dof(madgwick_handle,
0.6 * DEG2RAD,
1 * DEG2RAD,
3 * DEG2RAD,
0.43,
0.56,
1.3);
madgwick_get_quaternion(madgwick_handle, &quat_data);
float roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
float pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
float yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
printf("Roll: %f\nPitch: %f\nYaw: %f\n", roll, pitch, yaw);
return 0;
}

457
Core/Src/main.c Normal file
View File

@@ -0,0 +1,457 @@
/* 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"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* 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 ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM2_Init(void);
/* USER CODE BEGIN PFP */
/* 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_I2C1_Init();
MX_TIM1_Init();
MX_TIM2_Init();
/* USER CODE BEGIN 2 */
int state = 0;
state = MPU6000_Init(&hi2c1, 1000);
if(!state){
HAL_GPIO_WritePin(GPIOC, LED_Pin, 1);
}
#ifndef USE_CRSF
//SBUS_Init(&huart1, &htim1, &htim2, &htim3, &htim14);
#else
//CRSF_Init(&huart1, &htim1, &htim2, &htim3, &htim14);
#endif
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
//#ifndef USE_CRSF
// SBUS_Check_State();
//#else
// CRSF_Check_State();
//#endif
IMU_Check_State();
/* 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.HSIDiv = RCC_HSI_DIV1;
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 = 8;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
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_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief I2C1 Initialization Function
* @param None
* @retval None
*/
static void MX_I2C1_Init(void)
{
/* USER CODE BEGIN I2C1_Init 0 */
/* USER CODE END I2C1_Init 0 */
/* USER CODE BEGIN I2C1_Init 1 */
/* USER CODE END I2C1_Init 1 */
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x00303D5D;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/** Configure Analogue filter
*/
if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_DISABLE) != HAL_OK)
{
Error_Handler();
}
/** Configure Digital filter
*/
if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C1_Init 2 */
/* USER CODE END I2C1_Init 2 */
}
/**
* @brief TIM1 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM1_Init(void)
{
/* USER CODE BEGIN TIM1_Init 0 */
/* USER CODE END TIM1_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
/* USER CODE BEGIN TIM1_Init 1 */
/* USER CODE END TIM1_Init 1 */
htim1.Instance = TIM1;
htim1.Init.Prescaler = 63;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 19999;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
{
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();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 1500;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
{
Error_Handler();
}
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
sBreakDeadTimeConfig.DeadTime = 0;
sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
sBreakDeadTimeConfig.BreakFilter = 0;
sBreakDeadTimeConfig.BreakAFMode = TIM_BREAK_AFMODE_INPUT;
sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
sBreakDeadTimeConfig.Break2Filter = 0;
sBreakDeadTimeConfig.Break2AFMode = TIM_BREAK_AFMODE_INPUT;
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM1_Init 2 */
/* USER CODE END TIM1_Init 2 */
HAL_TIM_MspPostInit(&htim1);
}
/**
* @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_ClockConfigTypeDef sClockSourceConfig = {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 = 63;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 19999;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
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 = 1500;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
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();
}
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/**
* @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_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : LED_Pin */
GPIO_InitStruct.Pin = LED_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
/*void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){
#ifndef USE_CRSF
SBUS_UART_Callback(huart);
#else
CRSF_UART_Callback(huart);
#endif
}*/
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c){
MPU6000_I2C_CallbackFunc(hi2c);
}
/* USER CODE END 4 */
/**
* @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 */
}
#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 */

40
Core/Src/stabilize.c Normal file
View File

@@ -0,0 +1,40 @@
/*
* stabilize.c
*
* Created on: Aug 9, 2025
* Author: angoosh
*/
#include "stabilize.h"
Stabilize_Typedef STAB;
void Stabilize_init(){
STAB.pitch_gain = 1;
STAB.roll_gain = 1;
STAB.yaw_gain = 1;
}
int Stabilize_Roll(int servo){
int stab_servo = 0;
stab_servo = servo + (IMU.roll * STAB.roll_gain);
return stab_servo;
}
int Stabilize_Pitch(int servo){
int stab_servo = 0;
stab_servo = servo + (IMU.pitch * STAB.pitch_gain);
return stab_servo;
}
int Stabilize_Yaw(int servo){
int stab_servo = 0;
stab_servo = servo + (IMU.yaw * STAB.yaw_gain);
return stab_servo;
}

293
Core/Src/stm32g0xx_hal_msp.c Normal file
View File

@@ -0,0 +1,293 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g0xx_hal_msp.c
* @brief This file provides code for the MSP Initialization
* and de-Initialization codes.
******************************************************************************
* @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"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN Define */
/* USER CODE END Define */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN Macro */
/* USER CODE END Macro */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* External functions --------------------------------------------------------*/
/* USER CODE BEGIN ExternalFunctions */
/* USER CODE END ExternalFunctions */
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
/**
* Initializes the Global MSP.
*/
void HAL_MspInit(void)
{
/* USER CODE BEGIN MspInit 0 */
/* USER CODE END MspInit 0 */
__HAL_RCC_SYSCFG_CLK_ENABLE();
__HAL_RCC_PWR_CLK_ENABLE();
/* System interrupt init*/
/* USER CODE BEGIN MspInit 1 */
/* USER CODE END MspInit 1 */
}
/**
* @brief I2C MSP Initialization
* This function configures the hardware resources used in this example
* @param hi2c: I2C handle pointer
* @retval None
*/
void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
if(hi2c->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspInit 0 */
/* USER CODE END I2C1_MspInit 0 */
/** Initializes the peripherals clocks
*/
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1;
PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
__HAL_RCC_GPIOB_CLK_ENABLE();
/**I2C1 GPIO Configuration
PB8 ------> I2C1_SCL
PB9 ------> I2C1_SDA
*/
GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF6_I2C1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Peripheral clock enable */
__HAL_RCC_I2C1_CLK_ENABLE();
/* I2C1 interrupt Init */
HAL_NVIC_SetPriority(I2C1_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(I2C1_IRQn);
/* USER CODE BEGIN I2C1_MspInit 1 */
/* USER CODE END I2C1_MspInit 1 */
}
}
/**
* @brief I2C MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param hi2c: I2C handle pointer
* @retval None
*/
void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c)
{
if(hi2c->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspDeInit 0 */
/* USER CODE END I2C1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_I2C1_CLK_DISABLE();
/**I2C1 GPIO Configuration
PB8 ------> I2C1_SCL
PB9 ------> I2C1_SDA
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8);
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9);
/* I2C1 interrupt DeInit */
HAL_NVIC_DisableIRQ(I2C1_IRQn);
/* USER CODE BEGIN I2C1_MspDeInit 1 */
/* USER CODE END I2C1_MspDeInit 1 */
}
}
/**
* @brief TIM_Base MSP Initialization
* This function configures the hardware resources used in this example
* @param htim_base: TIM_Base handle pointer
* @retval None
*/
void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* htim_base)
{
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
if(htim_base->Instance==TIM1)
{
/* USER CODE BEGIN TIM1_MspInit 0 */
/* USER CODE END TIM1_MspInit 0 */
/** Initializes the peripherals clocks
*/
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_TIM1;
PeriphClkInit.Tim1ClockSelection = RCC_TIM1CLKSOURCE_PCLK1;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
/* Peripheral clock enable */
__HAL_RCC_TIM1_CLK_ENABLE();
/* USER CODE BEGIN TIM1_MspInit 1 */
/* USER CODE END TIM1_MspInit 1 */
}
else if(htim_base->Instance==TIM2)
{
/* USER CODE BEGIN TIM2_MspInit 0 */
/* USER CODE END TIM2_MspInit 0 */
/* Peripheral clock enable */
__HAL_RCC_TIM2_CLK_ENABLE();
/* USER CODE BEGIN TIM2_MspInit 1 */
/* USER CODE END TIM2_MspInit 1 */
}
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef* htim)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(htim->Instance==TIM1)
{
/* USER CODE BEGIN TIM1_MspPostInit 0 */
/* USER CODE END TIM1_MspPostInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
/**TIM1 GPIO Configuration
PA8 ------> TIM1_CH1
PA11 [PA9] ------> TIM1_CH4
*/
GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USER CODE BEGIN TIM1_MspPostInit 1 */
/* USER CODE END TIM1_MspPostInit 1 */
}
else if(htim->Instance==TIM2)
{
/* USER CODE BEGIN TIM2_MspPostInit 0 */
/* USER CODE END TIM2_MspPostInit 0 */
__HAL_RCC_GPIOA_CLK_ENABLE();
/**TIM2 GPIO Configuration
PA0 ------> TIM2_CH1
PA1 ------> TIM2_CH2
PA2 ------> TIM2_CH3
PA3 ------> TIM2_CH4
*/
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM2;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USER CODE BEGIN TIM2_MspPostInit 1 */
/* USER CODE END TIM2_MspPostInit 1 */
}
}
/**
* @brief TIM_Base MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param htim_base: TIM_Base handle pointer
* @retval None
*/
void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* htim_base)
{
if(htim_base->Instance==TIM1)
{
/* USER CODE BEGIN TIM1_MspDeInit 0 */
/* USER CODE END TIM1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM1_CLK_DISABLE();
/* USER CODE BEGIN TIM1_MspDeInit 1 */
/* USER CODE END TIM1_MspDeInit 1 */
}
else if(htim_base->Instance==TIM2)
{
/* USER CODE BEGIN TIM2_MspDeInit 0 */
/* USER CODE END TIM2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM2_CLK_DISABLE();
/* USER CODE BEGIN TIM2_MspDeInit 1 */
/* USER CODE END TIM2_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

163
Core/Src/stm32g0xx_it.c Normal file
View File

@@ -0,0 +1,163 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32g0xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @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 "stm32g0xx_it.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c1;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex-M0+ Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles Non maskable interrupt.
*/
void NMI_Handler(void)
{
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
while (1)
{
}
/* USER CODE END NonMaskableInt_IRQn 1 */
}
/**
* @brief This function handles Hard fault interrupt.
*/
void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
}
/**
* @brief This function handles System service call via SWI instruction.
*/
void SVC_Handler(void)
{
/* USER CODE BEGIN SVC_IRQn 0 */
/* USER CODE END SVC_IRQn 0 */
/* USER CODE BEGIN SVC_IRQn 1 */
/* USER CODE END SVC_IRQn 1 */
}
/**
* @brief This function handles Pendable request for system service.
*/
void PendSV_Handler(void)
{
/* USER CODE BEGIN PendSV_IRQn 0 */
/* USER CODE END PendSV_IRQn 0 */
/* USER CODE BEGIN PendSV_IRQn 1 */
/* USER CODE END PendSV_IRQn 1 */
}
/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 */
HAL_IncTick();
/* USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
/******************************************************************************/
/* STM32G0xx Peripheral Interrupt Handlers */
/* Add here the Interrupt Handlers for the used peripherals. */
/* For the available peripheral interrupt handler names, */
/* please refer to the startup file (startup_stm32g0xx.s). */
/******************************************************************************/
/**
* @brief This function handles I2C1 event global interrupt / I2C1 wake-up interrupt through EXTI line 23.
*/
void I2C1_IRQHandler(void)
{
/* USER CODE BEGIN I2C1_IRQn 0 */
/* USER CODE END I2C1_IRQn 0 */
if (hi2c1.Instance->ISR & (I2C_FLAG_BERR | I2C_FLAG_ARLO | I2C_FLAG_OVR)) {
HAL_I2C_ER_IRQHandler(&hi2c1);
} else {
HAL_I2C_EV_IRQHandler(&hi2c1);
}
/* USER CODE BEGIN I2C1_IRQn 1 */
/* USER CODE END I2C1_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

176
Core/Src/syscalls.c Normal file
View File

@@ -0,0 +1,176 @@
/**
******************************************************************************
* @file syscalls.c
* @author Auto-generated by STM32CubeIDE
* @brief STM32CubeIDE Minimal System calls file
*
* For more information about which c-functions
* need which of these lowlevel functions
* please consult the Newlib libc-manual
******************************************************************************
* @attention
*
* Copyright (c) 2020-2023 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.
*
******************************************************************************
*/
/* Includes */
#include <sys/stat.h>
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>
#include <sys/times.h>
/* Variables */
extern int __io_putchar(int ch) __attribute__((weak));
extern int __io_getchar(void) __attribute__((weak));
char *__env[1] = { 0 };
char **environ = __env;
/* Functions */
void initialise_monitor_handles()
{
}
int _getpid(void)
{
return 1;
}
int _kill(int pid, int sig)
{
(void)pid;
(void)sig;
errno = EINVAL;
return -1;
}
void _exit (int status)
{
_kill(status, -1);
while (1) {} /* Make sure we hang here */
}
__attribute__((weak)) int _read(int file, char *ptr, int len)
{
(void)file;
int DataIdx;
for (DataIdx = 0; DataIdx < len; DataIdx++)
{
*ptr++ = __io_getchar();
}
return len;
}
__attribute__((weak)) int _write(int file, char *ptr, int len)
{
(void)file;
int DataIdx;
for (DataIdx = 0; DataIdx < len; DataIdx++)
{
__io_putchar(*ptr++);
}
return len;
}
int _close(int file)
{
(void)file;
return -1;
}
int _fstat(int file, struct stat *st)
{
(void)file;
st->st_mode = S_IFCHR;
return 0;
}
int _isatty(int file)
{
(void)file;
return 1;
}
int _lseek(int file, int ptr, int dir)
{
(void)file;
(void)ptr;
(void)dir;
return 0;
}
int _open(char *path, int flags, ...)
{
(void)path;
(void)flags;
/* Pretend like we always fail */
return -1;
}
int _wait(int *status)
{
(void)status;
errno = ECHILD;
return -1;
}
int _unlink(char *name)
{
(void)name;
errno = ENOENT;
return -1;
}
int _times(struct tms *buf)
{
(void)buf;
return -1;
}
int _stat(char *file, struct stat *st)
{
(void)file;
st->st_mode = S_IFCHR;
return 0;
}
int _link(char *old, char *new)
{
(void)old;
(void)new;
errno = EMLINK;
return -1;
}
int _fork(void)
{
errno = EAGAIN;
return -1;
}
int _execve(char *name, char **argv, char **env)
{
(void)name;
(void)argv;
(void)env;
errno = ENOMEM;
return -1;
}

79
Core/Src/sysmem.c Normal file
View File

@@ -0,0 +1,79 @@
/**
******************************************************************************
* @file sysmem.c
* @author Generated by STM32CubeIDE
* @brief STM32CubeIDE System Memory calls file
*
* For more information about which C functions
* need which of these lowlevel functions
* please consult the newlib libc manual
******************************************************************************
* @attention
*
* Copyright (c) 2023 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.
*
******************************************************************************
*/
/* Includes */
#include <errno.h>
#include <stdint.h>
/**
* Pointer to the current high watermark of the heap usage
*/
static uint8_t *__sbrk_heap_end = NULL;
/**
* @brief _sbrk() allocates memory to the newlib heap and is used by malloc
* and others from the C library
*
* @verbatim
* ############################################################################
* # .data # .bss # newlib heap # MSP stack #
* # # # # Reserved by _Min_Stack_Size #
* ############################################################################
* ^-- RAM start ^-- _end _estack, RAM end --^
* @endverbatim
*
* This implementation starts allocating at the '_end' linker symbol
* The '_Min_Stack_Size' linker symbol reserves a memory for the MSP stack
* The implementation considers '_estack' linker symbol to be RAM end
* NOTE: If the MSP stack, at any point during execution, grows larger than the
* reserved size, please increase the '_Min_Stack_Size'.
*
* @param incr Memory size
* @return Pointer to allocated memory
*/
void *_sbrk(ptrdiff_t incr)
{
extern uint8_t _end; /* Symbol defined in the linker script */
extern uint8_t _estack; /* Symbol defined in the linker script */
extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */
const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;
const uint8_t *max_heap = (uint8_t *)stack_limit;
uint8_t *prev_heap_end;
/* Initialize heap end at first call */
if (NULL == __sbrk_heap_end)
{
__sbrk_heap_end = &_end;
}
/* Protect heap from growing into the reserved MSP stack */
if (__sbrk_heap_end + incr > max_heap)
{
errno = ENOMEM;
return (void *)-1;
}
prev_heap_end = __sbrk_heap_end;
__sbrk_heap_end += incr;
return (void *)prev_heap_end;
}

302
Core/Src/system_stm32g0xx.c Normal file
View File

@@ -0,0 +1,302 @@
/**
******************************************************************************
* @file system_stm32g0xx.c
* @author MCD Application Team
* @brief CMSIS Cortex-M0+ Device Peripheral Access Layer System Source File
*
* This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32g0xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
* After each device reset the HSI (8 MHz then 16 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32g0xx.s" file, to
* configure the system clock before to branch to main program.
*
* This file configures the system clock as follows:
*=============================================================================
*-----------------------------------------------------------------------------
* System Clock source | HSI
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 16000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 16000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB Prescaler | 1
*-----------------------------------------------------------------------------
* HSI Division factor | 1
*-----------------------------------------------------------------------------
* PLL_M | 1
*-----------------------------------------------------------------------------
* PLL_N | 8
*-----------------------------------------------------------------------------
* PLL_P | 7
*-----------------------------------------------------------------------------
* PLL_Q | 2
*-----------------------------------------------------------------------------
* PLL_R | 2
*-----------------------------------------------------------------------------
* Require 48MHz for RNG | Disabled
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* Copyright (c) 2018-2021 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.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32g0xx_system
* @{
*/
/** @addtogroup STM32G0xx_System_Private_Includes
* @{
*/
#include "stm32g0xx.h"
#if !defined (HSE_VALUE)
#define HSE_VALUE (8000000UL) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSI_VALUE)
#define HSI_VALUE (16000000UL) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
#if !defined (LSI_VALUE)
#define LSI_VALUE (32000UL) /*!< Value of LSI in Hz*/
#endif /* LSI_VALUE */
#if !defined (LSE_VALUE)
#define LSE_VALUE (32768UL) /*!< Value of LSE in Hz*/
#endif /* LSE_VALUE */
/**
* @}
*/
/** @addtogroup STM32G0xx_System_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @addtogroup STM32G0xx_System_Private_Defines
* @{
*/
/************************* Miscellaneous Configuration ************************/
/* Note: Following vector table addresses must be defined in line with linker
configuration. */
/*!< Uncomment the following line if you need to relocate the vector table
anywhere in Flash or Sram, else the vector table is kept at the automatic
remap of boot address selected */
/* #define USER_VECT_TAB_ADDRESS */
#if defined(USER_VECT_TAB_ADDRESS)
/*!< Uncomment the following line if you need to relocate your vector Table
in Sram else user remap will be done in Flash. */
/* #define VECT_TAB_SRAM */
#if defined(VECT_TAB_SRAM)
#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
#else
#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field.
This value must be a multiple of 0x200. */
#endif /* VECT_TAB_SRAM */
#endif /* USER_VECT_TAB_ADDRESS */
/******************************************************************************/
/**
* @}
*/
/** @addtogroup STM32G0xx_System_Private_Macros
* @{
*/
/**
* @}
*/
/** @addtogroup STM32G0xx_System_Private_Variables
* @{
*/
/* The SystemCoreClock variable is updated in three ways:
1) by calling CMSIS function SystemCoreClockUpdate()
2) by calling HAL API function HAL_RCC_GetHCLKFreq()
3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
Note: If you use this function to configure the system clock; then there
is no need to call the 2 first functions listed above, since SystemCoreClock
variable is updated automatically.
*/
uint32_t SystemCoreClock = 16000000UL;
const uint32_t AHBPrescTable[16UL] = {0UL, 0UL, 0UL, 0UL, 0UL, 0UL, 0UL, 0UL, 1UL, 2UL, 3UL, 4UL, 6UL, 7UL, 8UL, 9UL};
const uint32_t APBPrescTable[8UL] = {0UL, 0UL, 0UL, 0UL, 1UL, 2UL, 3UL, 4UL};
/**
* @}
*/
/** @addtogroup STM32G0xx_System_Private_FunctionPrototypes
* @{
*/
/**
* @}
*/
/** @addtogroup STM32G0xx_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* Configure the Vector Table location -------------------------------------*/
#if defined(USER_VECT_TAB_ADDRESS)
SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation */
#endif /* USER_VECT_TAB_ADDRESS */
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**) / HSI division factor
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***)
*
* - If SYSCLK source is LSI, SystemCoreClock will contain the LSI_VALUE
*
* - If SYSCLK source is LSE, SystemCoreClock will contain the LSE_VALUE
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (**) HSI_VALUE is a constant defined in stm32g0xx_hal_conf.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (***) HSE_VALUE is a constant defined in stm32g0xx_hal_conf.h file (default value
* 8 MHz), user has to ensure that HSE_VALUE is same as the real
* frequency of the crystal used. Otherwise, this function may
* have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate(void)
{
uint32_t tmp;
uint32_t pllvco;
uint32_t pllr;
uint32_t pllsource;
uint32_t pllm;
uint32_t hsidiv;
/* Get SYSCLK source -------------------------------------------------------*/
switch (RCC->CFGR & RCC_CFGR_SWS)
{
case RCC_CFGR_SWS_0: /* HSE used as system clock */
SystemCoreClock = HSE_VALUE;
break;
case (RCC_CFGR_SWS_1 | RCC_CFGR_SWS_0): /* LSI used as system clock */
SystemCoreClock = LSI_VALUE;
break;
case RCC_CFGR_SWS_2: /* LSE used as system clock */
SystemCoreClock = LSE_VALUE;
break;
case RCC_CFGR_SWS_1: /* PLL used as system clock */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLLM) * PLLN
SYSCLK = PLL_VCO / PLLR
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> RCC_PLLCFGR_PLLM_Pos) + 1UL;
if(pllsource == 0x03UL) /* HSE used as PLL clock source */
{
pllvco = (HSE_VALUE / pllm);
}
else /* HSI used as PLL clock source */
{
pllvco = (HSI_VALUE / pllm);
}
pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> RCC_PLLCFGR_PLLN_Pos);
pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> RCC_PLLCFGR_PLLR_Pos) + 1UL);
SystemCoreClock = pllvco/pllr;
break;
case 0x00000000U: /* HSI used as system clock */
default: /* HSI used as system clock */
hsidiv = (1UL << ((READ_BIT(RCC->CR, RCC_CR_HSIDIV))>> RCC_CR_HSIDIV_Pos));
SystemCoreClock = (HSI_VALUE/hsidiv);
break;
}
/* Compute HCLK clock frequency --------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> RCC_CFGR_HPRE_Pos)];
/* HCLK clock frequency */
SystemCoreClock >>= tmp;
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

287
Core/Startup/startup_stm32g031f8px.s Normal file
View File

@@ -0,0 +1,287 @@
/**
******************************************************************************
* @file startup_stm32g031xx.s
* @author MCD Application Team
* @brief STM32G031xx devices vector table GCC toolchain.
* This module performs:
* - Set the initial SP
* - Set the initial PC == Reset_Handler,
* - Set the vector table entries with the exceptions ISR address
* - Branches to main in the C library (which eventually
* calls main()).
* After Reset the Cortex-M0+ processor is in Thread mode,
* priority is Privileged, and the Stack is set to Main.
******************************************************************************
* @attention
*
* Copyright (c) 2018-2021 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.
*
******************************************************************************
*/
.syntax unified
.cpu cortex-m0plus
.fpu softvfp
.thumb
.global g_pfnVectors
.global Default_Handler
/* start address for the initialization values of the .data section.
defined in linker script */
.word _sidata
/* start address for the .data section. defined in linker script */
.word _sdata
/* end address for the .data section. defined in linker script */
.word _edata
/* start address for the .bss section. defined in linker script */
.word _sbss
/* end address for the .bss section. defined in linker script */
.word _ebss
/**
* @brief This is the code that gets called when the processor first
* starts execution following a reset event. Only the absolutely
* necessary set is performed, after which the application
* supplied main() routine is called.
* @param None
* @retval None
*/
.section .text.Reset_Handler
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
ldr r0, =_estack
mov sp, r0 /* set stack pointer */
/* Call the clock system initialization function.*/
bl SystemInit
/* Copy the data segment initializers from flash to SRAM */
ldr r0, =_sdata
ldr r1, =_edata
ldr r2, =_sidata
movs r3, #0
b LoopCopyDataInit
CopyDataInit:
ldr r4, [r2, r3]
str r4, [r0, r3]
adds r3, r3, #4
LoopCopyDataInit:
adds r4, r0, r3
cmp r4, r1
bcc CopyDataInit
/* Zero fill the bss segment. */
ldr r2, =_sbss
ldr r4, =_ebss
movs r3, #0
b LoopFillZerobss
FillZerobss:
str r3, [r2]
adds r2, r2, #4
LoopFillZerobss:
cmp r2, r4
bcc FillZerobss
/* Call static constructors */
bl __libc_init_array
/* Call the application s entry point.*/
bl main
LoopForever:
b LoopForever
.size Reset_Handler, .-Reset_Handler
/**
* @brief This is the code that gets called when the processor receives an
* unexpected interrupt. This simply enters an infinite loop, preserving
* the system state for examination by a debugger.
*
* @param None
* @retval None
*/
.section .text.Default_Handler,"ax",%progbits
Default_Handler:
Infinite_Loop:
b Infinite_Loop
.size Default_Handler, .-Default_Handler
/******************************************************************************
*
* The minimal vector table for a Cortex M0. Note that the proper constructs
* must be placed on this to ensure that it ends up at physical address
* 0x0000.0000.
*
******************************************************************************/
.section .isr_vector,"a",%progbits
.type g_pfnVectors, %object
g_pfnVectors:
.word _estack
.word Reset_Handler
.word NMI_Handler
.word HardFault_Handler
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word 0
.word SVC_Handler
.word 0
.word 0
.word PendSV_Handler
.word SysTick_Handler
.word WWDG_IRQHandler /* Window WatchDog */
.word PVD_IRQHandler /* PVD through EXTI Line detect */
.word RTC_TAMP_IRQHandler /* RTC through the EXTI line */
.word FLASH_IRQHandler /* FLASH */
.word RCC_IRQHandler /* RCC */
.word EXTI0_1_IRQHandler /* EXTI Line 0 and 1 */
.word EXTI2_3_IRQHandler /* EXTI Line 2 and 3 */
.word EXTI4_15_IRQHandler /* EXTI Line 4 to 15 */
.word 0 /* reserved */
.word DMA1_Channel1_IRQHandler /* DMA1 Channel 1 */
.word DMA1_Channel2_3_IRQHandler /* DMA1 Channel 2 and Channel 3 */
.word DMA1_Ch4_5_DMAMUX1_OVR_IRQHandler /* DMA1 Channel 4 to Channel 5, DMAMUX1 overrun */
.word ADC1_IRQHandler /* ADC1 */
.word TIM1_BRK_UP_TRG_COM_IRQHandler /* TIM1 Break, Update, Trigger and Commutation */
.word TIM1_CC_IRQHandler /* TIM1 Capture Compare */
.word TIM2_IRQHandler /* TIM2 */
.word TIM3_IRQHandler /* TIM3 */
.word LPTIM1_IRQHandler /* LPTIM1 */
.word LPTIM2_IRQHandler /* LPTIM2 */
.word TIM14_IRQHandler /* TIM14 */
.word 0 /* reserved */
.word TIM16_IRQHandler /* TIM16 */
.word TIM17_IRQHandler /* TIM17 */
.word I2C1_IRQHandler /* I2C1 */
.word I2C2_IRQHandler /* I2C2 */
.word SPI1_IRQHandler /* SPI1 */
.word SPI2_IRQHandler /* SPI2 */
.word USART1_IRQHandler /* USART1 */
.word USART2_IRQHandler /* USART2 */
.word LPUART1_IRQHandler /* LPUART1 */
.word 0 /* reserved */
.size g_pfnVectors, .-g_pfnVectors
/*******************************************************************************
*
* Provide weak aliases for each Exception handler to the Default_Handler.
* As they are weak aliases, any function with the same name will override
* this definition.
*
*******************************************************************************/
.weak NMI_Handler
.thumb_set NMI_Handler,Default_Handler
.weak HardFault_Handler
.thumb_set HardFault_Handler,Default_Handler
.weak SVC_Handler
.thumb_set SVC_Handler,Default_Handler
.weak PendSV_Handler
.thumb_set PendSV_Handler,Default_Handler
.weak SysTick_Handler
.thumb_set SysTick_Handler,Default_Handler
.weak WWDG_IRQHandler
.thumb_set WWDG_IRQHandler,Default_Handler
.weak PVD_IRQHandler
.thumb_set PVD_IRQHandler,Default_Handler
.weak RTC_TAMP_IRQHandler
.thumb_set RTC_TAMP_IRQHandler,Default_Handler
.weak FLASH_IRQHandler
.thumb_set FLASH_IRQHandler,Default_Handler
.weak RCC_IRQHandler
.thumb_set RCC_IRQHandler,Default_Handler
.weak EXTI0_1_IRQHandler
.thumb_set EXTI0_1_IRQHandler,Default_Handler
.weak EXTI2_3_IRQHandler
.thumb_set EXTI2_3_IRQHandler,Default_Handler
.weak EXTI4_15_IRQHandler
.thumb_set EXTI4_15_IRQHandler,Default_Handler
.weak DMA1_Channel1_IRQHandler
.thumb_set DMA1_Channel1_IRQHandler,Default_Handler
.weak DMA1_Channel2_3_IRQHandler
.thumb_set DMA1_Channel2_3_IRQHandler,Default_Handler
.weak DMA1_Ch4_5_DMAMUX1_OVR_IRQHandler
.thumb_set DMA1_Ch4_5_DMAMUX1_OVR_IRQHandler,Default_Handler
.weak ADC1_IRQHandler
.thumb_set ADC1_IRQHandler,Default_Handler
.weak TIM1_BRK_UP_TRG_COM_IRQHandler
.thumb_set TIM1_BRK_UP_TRG_COM_IRQHandler,Default_Handler
.weak TIM1_CC_IRQHandler
.thumb_set TIM1_CC_IRQHandler,Default_Handler
.weak TIM2_IRQHandler
.thumb_set TIM2_IRQHandler,Default_Handler
.weak TIM3_IRQHandler
.thumb_set TIM3_IRQHandler,Default_Handler
.weak LPTIM1_IRQHandler
.thumb_set LPTIM1_IRQHandler,Default_Handler
.weak LPTIM2_IRQHandler
.thumb_set LPTIM2_IRQHandler,Default_Handler
.weak TIM14_IRQHandler
.thumb_set TIM14_IRQHandler,Default_Handler
.weak TIM16_IRQHandler
.thumb_set TIM16_IRQHandler,Default_Handler
.weak TIM17_IRQHandler
.thumb_set TIM17_IRQHandler,Default_Handler
.weak I2C1_IRQHandler
.thumb_set I2C1_IRQHandler,Default_Handler
.weak I2C2_IRQHandler
.thumb_set I2C2_IRQHandler,Default_Handler
.weak SPI1_IRQHandler
.thumb_set SPI1_IRQHandler,Default_Handler
.weak SPI2_IRQHandler
.thumb_set SPI2_IRQHandler,Default_Handler
.weak USART1_IRQHandler
.thumb_set USART1_IRQHandler,Default_Handler
.weak USART2_IRQHandler
.thumb_set USART2_IRQHandler,Default_Handler
.weak LPUART1_IRQHandler
.thumb_set LPUART1_IRQHandler,Default_Handler