pima/Trinity-DevPCB-V1.0
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Trinity Devboard PCB V1.0 Firmware. FreeRTOS is setup and the MCU reads IMU data over SPI fand Magnetometer data over I2C, each with a seperate task. Sensordata is then run though MadgwickAHRS and send over USB as serial packet data to use in trinity visualizer. Bare minimum functionality works and is replicated from the first prototype.

Browse Source
This commit is contained in:
pima
2026-09-13 19:51:03 +02:00
commit 7ee381de58
1630 changed files with 632404 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Core/Src/MadgwickAHRS.c
+252
View File
@@ -0,0 +1,252 @@
//=====================================================================================================
// MadgwickAHRS.c
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
//
//=====================================================================================================
//---------------------------------------------------------------------------------------------------
// Header files
#include "MadgwickAHRS.h"
#include <math.h>
#include <stdio.h>
//---------------------------------------------------------------------------------------------------
// Definitions
//#define sampleFreq 512.0f // sample frequency in Hz
//#define betaDef 0.1f // 2 * proportional gain
//---------------------------------------------------------------------------------------------------
// Variable definitions
//volatile float beta = betaDef; // 2 * proportional gain (Kp)
//volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
//---------------------------------------------------------------------------------------------------
// Function declarations
float invSqrt(float x);
//====================================================================================================
// Functions
void MadgwickAHRS_init (MadgwickAHRS_Filter* filter, float beta, float sample_freq) {
filter->q[0] = 1.0f; // qw
filter->q[1] = 0.0f; // qx
filter->q[2] = 0.0f; // qy
filter->q[3] = 0.0f; // qz
filter->beta = beta;
filter->sample_freq = sample_freq;
}
//---------------------------------------------------------------------------------------------------
// AHRS algorithm update
void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
float recipNorm;
float s0, s1, s2, s3;
float qDot[4];
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;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRS_update_IMU(filter, gx, gy, gz, ax, ay, az);
return;
}
// Rate of change of quaternion from gyroscope
qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * 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 * q[0] * mx;
_2q0my = 2.0f * q[0] * my;
_2q0mz = 2.0f * q[0] * mz;
_2q1mx = 2.0f * q[1] * mx;
_2q0 = 2.0f * q[0];
_2q1 = 2.0f * q[1];
_2q2 = 2.0f * q[2];
_2q3 = 2.0f * q[3];
_2q0q2 = 2.0f * q[0] * q[2];
_2q2q3 = 2.0f * q[2] * q[3];
q0q0 = q[0] * q[0];
q0q1 = q[0] * q[1];
q0q2 = q[0] * q[2];
q0q3 = q[0] * q[3];
q1q1 = q[1] * q[1];
q1q2 = q[1] * q[2];
q1q3 = q[1] * q[3];
q2q2 = q[2] * q[2];
q2q3 = q[2] * q[3];
q3q3 = q[3] * q[3];
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q[3] + _2q0mz * q[2] + mx * q1q1 + _2q1 * my * q[2] + _2q1 * mz * q[3] - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q[3] + my * q0q0 - _2q0mz * q[1] + _2q1mx * q[2] - my * q1q1 + my * q2q2 + _2q2 * mz * q[3] - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q[2] + _2q0my * q[1] + mz * q0q0 + _2q1mx * q[3] - mz * q1q1 + _2q2 * my * q[3] - 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 * q[2] * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q[3] + _2bz * q[1]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q[2] * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q[1] * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q[3] * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q[2] + _2bz * q[0]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q[3] - _4bz * q[1]) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q[2] * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q[2] - _2bz * q[0]) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q[1] + _2bz * q[3]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q[0] - _4bz * q[2]) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q[3] + _2bz * q[1]) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q[0] + _2bz * q[2]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q[1] * (_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
qDot[0] -= filter->beta * s0;
qDot[1] -= filter->beta * s1;
qDot[2] -= filter->beta * s2;
qDot[3] -= filter->beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q[0] += qDot[0] * (1.0f / filter->sample_freq);
q[1] += qDot[1] * (1.0f / filter->sample_freq);
q[2] += qDot[2] * (1.0f / filter->sample_freq);
q[3] += qDot[3] * (1.0f / filter->sample_freq);
// Normalise quaternion
recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
// Update filter state
filter->q[0] = q[0];
filter->q[1] = q[1];
filter->q[2] = q[2];
filter->q[3] = q[3];
}
//---------------------------------------------------------------------------------------------------
// IMU algorithm update
void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az) {
float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
float recipNorm;
float s0, s1, s2, s3;
float qDot[4];
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * 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 * q[0];
_2q1 = 2.0f * q[1];
_2q2 = 2.0f * q[2];
_2q3 = 2.0f * q[3];
_4q0 = 4.0f * q[0];
_4q1 = 4.0f * q[1];
_4q2 = 4.0f * q[2];
_8q1 = 8.0f * q[1];
_8q2 = 8.0f * q[2];
q0q0 = q[0] * q[0];
q1q1 = q[1] * q[1];
q2q2 = q[2] * q[2];
q3q3 = q[3] * q[3];
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q[3] - _2q1 * ax + 4.0f * q2q2 * q[3] - _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
qDot[0] -= filter->beta * s0;
qDot[1] -= filter->beta * s1;
qDot[2] -= filter->beta * s2;
qDot[3] -= filter->beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q[0] += qDot[0] * (1.0f / filter->sample_freq);
q[1] += qDot[1] * (1.0f / filter->sample_freq);
q[2] += qDot[2] * (1.0f / filter->sample_freq);
q[3] += qDot[3] * (1.0f / filter->sample_freq);
// Normalise quaternion
recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
// Update filter state
filter->q[0] = q[0];
filter->q[1] = q[1];
filter->q[2] = q[2];
filter->q[3] = q[3];
}
//---------------------------------------------------------------------------------------------------
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
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;
}
//====================================================================================================
// END OF CODE
//====================================================================================================
Core/Src/MadgwickAHRS.h
+37
View File
@@ -0,0 +1,37 @@
//=====================================================================================================
// MadgwickAHRS.h
//=====================================================================================================
//
// Implementation of Madgwick's IMU and AHRS algorithms.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
// Date Author Notes
// 29/09/2011 SOH Madgwick Initial release
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
//
//=====================================================================================================
#ifndef MadgwickAHRS_h
#define MadgwickAHRS_h
//----------------------------------------------------------------------------------------------------
// Variable declaration
//extern volatile float beta; // algorithm gain
//extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
typedef struct {
float q[4]; // Quaternion: [qw, qx, qy, qz]
float beta; // Filter gain (tunes correction strength)
float sample_freq; // Sampling frequency in Hz
} MadgwickAHRS_Filter;
//---------------------------------------------------------------------------------------------------
// Function declarations
void MadgwickAHRS_init (MadgwickAHRS_Filter* filter, float beta, float sample_freq);
void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az);
#endif
//=====================================================================================================
// End of file
//=====================================================================================================
Core/Src/app_debug.c
+409
View File
@@ -0,0 +1,409 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file app_debug.c
* @author MCD Application Team
* @brief Debug capabilities source file for STM32WPAN Middleware
******************************************************************************
* @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 ------------------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "app_common.h"
#include "app_debug.h"
#include "utilities_common.h"
#include "shci.h"
#include "tl.h"
#include "dbg_trace.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
typedef PACKED_STRUCT
{
GPIO_TypeDef* port;
uint16_t pin;
uint8_t enable;
uint8_t reserved;
} APPD_GpioConfig_t;
/* USER CODE END PTD */
/* Private defines -----------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define GPIO_NBR_OF_RF_SIGNALS 9
#define GPIO_CFG_NBR_OF_FEATURES 38
#define NBR_OF_TRACES_CONFIG_PARAMETERS 4
#define NBR_OF_GENERAL_CONFIG_PARAMETERS 4
/**
* THIS SHALL BE SET TO A VALUE DIFFERENT FROM 0 ONLY ON REQUEST FROM ST SUPPORT
*/
#define BLE_DTB_CFG 0
/**
* System Debug Options flags to be configured with:
* - SHCI_C2_DEBUG_OPTIONS_IPCORE_LP
* - SHCI_C2_DEBUG_OPTIONS_IPCORE_NO_LP
* - SHCI_C2_DEBUG_OPTIONS_CPU2_STOP_EN
* - SHCI_C2_DEBUG_OPTIONS_CPU2_STOP_DIS
* which are used to set following configuration bits:
* - bit 0: 0: IP BLE core in LP mode 1: IP BLE core in run mode (no LP supported)
* - bit 1: 0: CPU2 STOP mode Enable 1: CPU2 STOP mode Disable
* - bit [2-7]: bits reserved ( shall be set to 0)
*/
#define SYS_DBG_CFG1 (SHCI_C2_DEBUG_OPTIONS_IPCORE_LP | SHCI_C2_DEBUG_OPTIONS_CPU2_STOP_EN)
/* USER CODE END PD */
/* Private macros ------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static SHCI_C2_DEBUG_TracesConfig_t APPD_TracesConfig={0, 0, 0, 0};
PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static SHCI_C2_DEBUG_GeneralConfig_t APPD_GeneralConfig={BLE_DTB_CFG, SYS_DBG_CFG1, {0, 0}};
#ifdef CFG_DEBUG_TRACE_UART
#if(CFG_HW_LPUART1_ENABLED == 1)
extern void MX_LPUART1_UART_Init(void);
#endif
#if(CFG_HW_USART1_ENABLED == 1)
extern void MX_USART1_UART_Init(void);
#endif
#endif
/**
* THE DEBUG ON GPIO FOR CPU2 IS INTENDED TO BE USED ONLY ON REQUEST FROM ST SUPPORT
* It provides timing information on the CPU2 activity.
* All configuration of (port, pin) is supported for each features and can be selected by the user
* depending on the availability
*/
static const APPD_GpioConfig_t aGpioConfigList[GPIO_CFG_NBR_OF_FEATURES] =
{
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_ISR - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_STACK_TICK - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_CMD_PROCESS - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_ACL_DATA_PROCESS - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* SYS_CMD_PROCESS - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* RNG_PROCESS - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* NVM_PROCESS - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_GENERAL - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_BLE_CMD_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_BLE_EVT_TX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_BLE_ACL_DATA_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_SYS_CMD_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_SYS_EVT_TX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_CLI_CMD_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_OT_CMD_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_OT_ACK_TX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_CLI_ACK_TX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_MEM_MANAGER_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_TRACES_TX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* HARD_FAULT - Set on Entry / Reset on Exit */
/* From v1.1.1 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IP_CORE_LP_STATUS - Set on Entry / Reset on Exit */
/* From v1.2.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* END_OF_CONNECTION_EVENT - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* TIMER_SERVER_CALLBACK - Toggle on Entry */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* PES_ACTIVITY - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* MB_BLE_SEND_EVT - Set on Entry / Reset on Exit */
/* From v1.3.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_NO_DELAY - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_STACK_STORE_NVM_CB - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* NVMA_WRITE_ONGOING - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* NVMA_WRITE_COMPLETE - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* NVMA_CLEANUP - Set on Entry / Reset on Exit */
/* From v1.4.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* NVMA_START - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* FLASH_EOP - Set on Entry / Reset on Exit */
/* From v1.5.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* FLASH_WRITE - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* FLASH_ERASE - Set on Entry / Reset on Exit */
/* From v1.6.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_RESCHEDULE_EVENT - Set on Entry / Reset on Exit */
/* From v1.8.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_BLE_LLD_CMD_RX - Set on Entry / Reset on Exit */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* IPCC_BLE_LLD_ACK_TX - Set on Entry / Reset on Exit */
/* From v1.9.0 */
{ GPIOA, LL_GPIO_PIN_0, 0, 0}, /* BLE_ASYNCH_EVENT_NACKED - Set on Entry / Reset on Exit */
};
/**
* THE DEBUG ON GPIO FOR CPU2 IS INTENDED TO BE USED ONLY ON REQUEST FROM ST SUPPORT
* This table is relevant only for BLE
* It provides timing information on BLE RF activity.
* New signals may be allocated at any location when requested by ST
* The GPIO allocated to each signal depend on the BLE_DTB_CFG value and cannot be changed
*/
#if( BLE_DTB_CFG == 7)
static const APPD_GpioConfig_t aRfConfigList[GPIO_NBR_OF_RF_SIGNALS] =
{
{ GPIOB, LL_GPIO_PIN_2, 0, 0}, /* DTB10 - Tx/Rx SPI */
{ GPIOB, LL_GPIO_PIN_7, 0, 0}, /* DTB11 - Tx/Tx SPI Clk */
{ GPIOA, LL_GPIO_PIN_8, 0, 0}, /* DTB12 - Tx/Rx Ready & SPI Select */
{ GPIOA, LL_GPIO_PIN_9, 0, 0}, /* DTB13 - Tx/Rx Start */
{ GPIOA, LL_GPIO_PIN_10, 0, 0}, /* DTB14 - FSM0 */
{ GPIOA, LL_GPIO_PIN_11, 0, 0}, /* DTB15 - FSM1 */
{ GPIOB, LL_GPIO_PIN_8, 0, 0}, /* DTB16 - FSM2 */
{ GPIOB, LL_GPIO_PIN_11, 0, 0}, /* DTB17 - FSM3 */
{ GPIOB, LL_GPIO_PIN_10, 0, 0}, /* DTB18 - FSM4 */
};
#endif
/* USER CODE END PV */
/* Global variables ----------------------------------------------------------*/
/* USER CODE BEGIN GV */
/* USER CODE END GV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
static void APPD_SetCPU2GpioConfig( void );
static void APPD_BleDtbCfg( void );
/* USER CODE END PFP */
/* Functions Definition ------------------------------------------------------*/
void APPD_Init( void )
{
/* USER CODE BEGIN APPD_Init */
#if (CFG_DEBUGGER_SUPPORTED == 1)
/**
* Keep debugger enabled while in any low power mode
*/
HAL_DBGMCU_EnableDBGSleepMode();
HAL_DBGMCU_EnableDBGStopMode();
/***************** ENABLE DEBUGGER *************************************/
LL_EXTI_EnableIT_32_63(LL_EXTI_LINE_48);
#else
GPIO_InitTypeDef gpio_config = {0};
gpio_config.Pull = GPIO_NOPULL;
gpio_config.Mode = GPIO_MODE_ANALOG;
gpio_config.Pin = GPIO_PIN_15 | GPIO_PIN_14 | GPIO_PIN_13;
__HAL_RCC_GPIOA_CLK_ENABLE();
HAL_GPIO_Init(GPIOA, &gpio_config);
__HAL_RCC_GPIOA_CLK_DISABLE();
gpio_config.Pin = GPIO_PIN_4 | GPIO_PIN_3;
__HAL_RCC_GPIOB_CLK_ENABLE();
HAL_GPIO_Init(GPIOB, &gpio_config);
__HAL_RCC_GPIOB_CLK_DISABLE();
HAL_DBGMCU_DisableDBGSleepMode();
HAL_DBGMCU_DisableDBGStopMode();
HAL_DBGMCU_DisableDBGStandbyMode();
#endif /* (CFG_DEBUGGER_SUPPORTED == 1) */
#if(CFG_DEBUG_TRACE != 0)
DbgTraceInit();
#endif
APPD_SetCPU2GpioConfig( );
APPD_BleDtbCfg( );
/* USER CODE END APPD_Init */
return;
}
void APPD_EnableCPU2( void )
{
/* USER CODE BEGIN APPD_EnableCPU2 */
SHCI_C2_DEBUG_Init_Cmd_Packet_t DebugCmdPacket =
{
{{0,0,0}}, /**< Does not need to be initialized */
{(uint8_t *)aGpioConfigList,
(uint8_t *)&APPD_TracesConfig,
(uint8_t *)&APPD_GeneralConfig,
GPIO_CFG_NBR_OF_FEATURES,
NBR_OF_TRACES_CONFIG_PARAMETERS,
NBR_OF_GENERAL_CONFIG_PARAMETERS}
};
/**< Traces channel initialization */
TL_TRACES_Init( );
/** GPIO DEBUG Initialization */
SHCI_C2_DEBUG_Init( &DebugCmdPacket );
/* USER CODE END APPD_EnableCPU2 */
return;
}
/*************************************************************
*
* LOCAL FUNCTIONS
*
*************************************************************/
static void APPD_SetCPU2GpioConfig( void )
{
/* USER CODE BEGIN APPD_SetCPU2GpioConfig */
GPIO_InitTypeDef gpio_config = {0};
uint8_t local_loop;
uint16_t gpioa_pin_list;
uint16_t gpiob_pin_list;
uint16_t gpioc_pin_list;
gpioa_pin_list = 0;
gpiob_pin_list = 0;
gpioc_pin_list = 0;
for(local_loop = 0 ; local_loop < GPIO_CFG_NBR_OF_FEATURES; local_loop++)
{
if( aGpioConfigList[local_loop].enable != 0)
{
switch((uint32_t)aGpioConfigList[local_loop].port)
{
case (uint32_t)GPIOA:
gpioa_pin_list |= aGpioConfigList[local_loop].pin;
break;
case (uint32_t)GPIOB:
gpiob_pin_list |= aGpioConfigList[local_loop].pin;
break;
case (uint32_t)GPIOC:
gpioc_pin_list |= aGpioConfigList[local_loop].pin;
break;
default:
break;
}
}
}
gpio_config.Pull = GPIO_NOPULL;
gpio_config.Mode = GPIO_MODE_OUTPUT_PP;
gpio_config.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
if(gpioa_pin_list != 0)
{
gpio_config.Pin = gpioa_pin_list;
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_C2GPIOA_CLK_ENABLE();
HAL_GPIO_Init(GPIOA, &gpio_config);
HAL_GPIO_WritePin(GPIOA, gpioa_pin_list, GPIO_PIN_RESET);
}
if(gpiob_pin_list != 0)
{
gpio_config.Pin = gpiob_pin_list;
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_C2GPIOB_CLK_ENABLE();
HAL_GPIO_Init(GPIOB, &gpio_config);
HAL_GPIO_WritePin(GPIOB, gpiob_pin_list, GPIO_PIN_RESET);
}
if(gpioc_pin_list != 0)
{
gpio_config.Pin = gpioc_pin_list;
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_C2GPIOC_CLK_ENABLE();
HAL_GPIO_Init(GPIOC, &gpio_config);
HAL_GPIO_WritePin(GPIOC, gpioc_pin_list, GPIO_PIN_RESET);
}
/* USER CODE END APPD_SetCPU2GpioConfig */
return;
}
static void APPD_BleDtbCfg( void )
{
/* USER CODE BEGIN APPD_BleDtbCfg */
#if (BLE_DTB_CFG != 0)
GPIO_InitTypeDef gpio_config = {0};
uint8_t local_loop;
uint16_t gpioa_pin_list;
uint16_t gpiob_pin_list;
gpioa_pin_list = 0;
gpiob_pin_list = 0;
for(local_loop = 0 ; local_loop < GPIO_NBR_OF_RF_SIGNALS; local_loop++)
{
if( aRfConfigList[local_loop].enable != 0)
{
switch((uint32_t)aRfConfigList[local_loop].port)
{
case (uint32_t)GPIOA:
gpioa_pin_list |= aRfConfigList[local_loop].pin;
break;
case (uint32_t)GPIOB:
gpiob_pin_list |= aRfConfigList[local_loop].pin;
break;
default:
break;
}
}
}
gpio_config.Pull = GPIO_NOPULL;
gpio_config.Mode = GPIO_MODE_AF_PP;
gpio_config.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
gpio_config.Alternate = GPIO_AF6_RF_DTB7;
if(gpioa_pin_list != 0)
{
gpio_config.Pin = gpioa_pin_list;
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_C2GPIOA_CLK_ENABLE();
HAL_GPIO_Init(GPIOA, &gpio_config);
}
if(gpiob_pin_list != 0)
{
gpio_config.Pin = gpiob_pin_list;
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_C2GPIOB_CLK_ENABLE();
HAL_GPIO_Init(GPIOB, &gpio_config);
}
#endif
/* USER CODE END APPD_BleDtbCfg */
return;
}
/*************************************************************
*
* WRAP FUNCTIONS
*
*************************************************************/
#if(CFG_DEBUG_TRACE != 0)
void DbgOutputInit( void )
{
/* USER CODE BEGIN DbgOutputInit */
#ifdef CFG_DEBUG_TRACE_UART
if (CFG_DEBUG_TRACE_UART == hw_lpuart1)
{
#if(CFG_HW_LPUART1_ENABLED == 1)
MX_LPUART1_UART_Init();
#endif
}
else if (CFG_DEBUG_TRACE_UART == hw_uart1)
{
#if(CFG_HW_USART1_ENABLED == 1)
MX_USART1_UART_Init();
#endif
}
#endif
/* USER CODE END DbgOutputInit */
return;
}
void DbgOutputTraces( uint8_t *p_data, uint16_t size, void (*cb)(void) )
{
/* USER CODE BEGIN DbgOutputTraces */
HW_UART_Transmit_DMA(CFG_DEBUG_TRACE_UART, p_data, size, cb);
/* USER CODE END DbgOutputTraces */
return;
}
#endif
Core/Src/app_entry.c
+626
View File
@@ -0,0 +1,626 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file app_entry.c
* @author MCD Application Team
* @brief Entry point of the application
******************************************************************************
* @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 "app_common.h"
#include "main.h"
#include "app_entry.h"
#include "ble.h"
#include "tl.h"
#include "cmsis_os.h"
#include "shci_tl.h"
#include "stm32_lpm.h"
#include "app_debug.h"
#include "dbg_trace.h"
#include "shci.h"
#include "otp.h"
/* Private includes -----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
extern RTC_HandleTypeDef hrtc;
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private defines -----------------------------------------------------------*/
#define POOL_SIZE (CFG_TLBLE_EVT_QUEUE_LENGTH*4U*DIVC((sizeof(TL_PacketHeader_t) + TL_BLE_EVENT_FRAME_SIZE), 4U))
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macros ------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t EvtPool[POOL_SIZE];
PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static TL_CmdPacket_t SystemCmdBuffer;
PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t SystemSpareEvtBuffer[sizeof(TL_PacketHeader_t) + TL_EVT_HDR_SIZE + 255U];
PLACE_IN_SECTION("MB_MEM2") ALIGN(4) static uint8_t BleSpareEvtBuffer[sizeof(TL_PacketHeader_t) + TL_EVT_HDR_SIZE + 255];
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Global variables ----------------------------------------------------------*/
osMutexId_t MtxShciId;
osSemaphoreId_t SemShciId;
osThreadId_t ShciUserEvtProcessId;
const osThreadAttr_t ShciUserEvtProcess_attr = {
.name = CFG_SHCI_USER_EVT_PROCESS_NAME,
.attr_bits = CFG_SHCI_USER_EVT_PROCESS_ATTR_BITS,
.cb_mem = CFG_SHCI_USER_EVT_PROCESS_CB_MEM,
.cb_size = CFG_SHCI_USER_EVT_PROCESS_CB_SIZE,
.stack_mem = CFG_SHCI_USER_EVT_PROCESS_STACK_MEM,
.priority = CFG_SHCI_USER_EVT_PROCESS_PRIORITY,
.stack_size = CFG_SHCI_USER_EVT_PROCESS_STACK_SIZE
};
/* Private functions prototypes-----------------------------------------------*/
static void ShciUserEvtProcess(void *argument);
static void Config_HSE(void);
static void Reset_Device(void);
#if (CFG_HW_RESET_BY_FW == 1)
static void Reset_IPCC(void);
static void Reset_BackupDomain(void);
#endif /* CFG_HW_RESET_BY_FW == 1*/
static void System_Init(void);
static void SystemPower_Config(void);
static void appe_Tl_Init(void);
static void APPE_SysStatusNot(SHCI_TL_CmdStatus_t status);
static void APPE_SysUserEvtRx(void * pPayload);
static void APPE_SysEvtReadyProcessing(void * pPayload);
static void APPE_SysEvtError(void * pPayload);
static void Init_Rtc(void);
__WEAK void APP_BLE_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Functions Definition ------------------------------------------------------*/
void MX_APPE_Config(void)
{
/**
* The OPTVERR flag is wrongly set at power on
* It shall be cleared before using any HAL_FLASH_xxx() api
*/
__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_OPTVERR);
/**
* Reset some configurations so that the system behave in the same way
* when either out of nReset or Power On
*/
Reset_Device();
/* Configure HSE Tuning */
Config_HSE();
return;
}
void MX_APPE_Init(void)
{
System_Init(); /**< System initialization */
SystemPower_Config(); /**< Configure the system Power Mode */
HW_TS_Init(hw_ts_InitMode_Full, &hrtc); /**< Initialize the TimerServer */
/* USER CODE BEGIN APPE_Init_1 */
/* USER CODE END APPE_Init_1 */
appe_Tl_Init(); /* Initialize all transport layers */
/**
* From now, the application is waiting for the ready event (VS_HCI_C2_Ready)
* received on the system channel before starting the Stack
* This system event is received with APPE_SysUserEvtRx()
*/
/* USER CODE BEGIN APPE_Init_2 */
/* USER CODE END APPE_Init_2 */
return;
}
void Init_Smps(void)
{
#if (CFG_USE_SMPS != 0)
/**
* Configure and enable SMPS
*
* The SMPS configuration is not yet supported by CubeMx
* when SMPS output voltage is set to 1.4V, the RF output power is limited to 3.7dBm
* the SMPS output voltage shall be increased for higher RF output power
*/
LL_PWR_SMPS_SetStartupCurrent(LL_PWR_SMPS_STARTUP_CURRENT_80MA);
LL_PWR_SMPS_SetOutputVoltageLevel(LL_PWR_SMPS_OUTPUT_VOLTAGE_1V40);
LL_PWR_SMPS_Enable();
#endif /* CFG_USE_SMPS != 0 */
return;
}
void Init_Exti(void)
{
/* Enable IPCC(36), HSEM(38) wakeup interrupts on CPU1 */
LL_EXTI_EnableIT_32_63(LL_EXTI_LINE_36 | LL_EXTI_LINE_38);
return;
}
/* USER CODE BEGIN FD */
/* USER CODE END FD */
/*************************************************************
*
* LOCAL FUNCTIONS
*
*************************************************************/
static void Reset_Device(void)
{
#if (CFG_HW_RESET_BY_FW == 1)
Reset_BackupDomain();
Reset_IPCC();
#endif /* CFG_HW_RESET_BY_FW == 1 */
return;
}
#if (CFG_HW_RESET_BY_FW == 1)
static void Reset_BackupDomain(void)
{
if ((LL_RCC_IsActiveFlag_PINRST() != FALSE) && (LL_RCC_IsActiveFlag_SFTRST() == FALSE))
{
HAL_PWR_EnableBkUpAccess(); /**< Enable access to the RTC registers */
/**
* Write twice the value to flush the APB-AHB bridge
* This bit shall be written in the register before writing the next one
*/
HAL_PWR_EnableBkUpAccess();
__HAL_RCC_BACKUPRESET_FORCE();
__HAL_RCC_BACKUPRESET_RELEASE();
}
return;
}
static void Reset_IPCC(void)
{
LL_AHB3_GRP1_EnableClock(LL_AHB3_GRP1_PERIPH_IPCC);
LL_C1_IPCC_ClearFlag_CHx(
IPCC,
LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
| LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
LL_C2_IPCC_ClearFlag_CHx(
IPCC,
LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
| LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
LL_C1_IPCC_DisableTransmitChannel(
IPCC,
LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
| LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
LL_C2_IPCC_DisableTransmitChannel(
IPCC,
LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
| LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
LL_C1_IPCC_DisableReceiveChannel(
IPCC,
LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
| LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
LL_C2_IPCC_DisableReceiveChannel(
IPCC,
LL_IPCC_CHANNEL_1 | LL_IPCC_CHANNEL_2 | LL_IPCC_CHANNEL_3 | LL_IPCC_CHANNEL_4
| LL_IPCC_CHANNEL_5 | LL_IPCC_CHANNEL_6);
return;
}
#endif /* CFG_HW_RESET_BY_FW == 1 */
static void Config_HSE(void)
{
OTP_ID0_t * p_otp;
/**
* Read HSE_Tuning from OTP
*/
p_otp = (OTP_ID0_t *) OTP_Read(0);
if (p_otp)
{
LL_RCC_HSE_SetCapacitorTuning(p_otp->hse_tuning);
}
return;
}
static void System_Init(void)
{
Init_Smps();
Init_Exti();
Init_Rtc();
return;
}
static void Init_Rtc(void)
{
/* Disable RTC registers write protection */
LL_RTC_DisableWriteProtection(RTC);
LL_RTC_WAKEUP_SetClock(RTC, CFG_RTC_WUCKSEL_DIVIDER);
/* Enable RTC registers write protection */
LL_RTC_EnableWriteProtection(RTC);
return;
}
/**
* @brief Configure the system for power optimization
*
* @note This API configures the system to be ready for low power mode
*
* @param None
* @retval None
*/
static void SystemPower_Config(void)
{
/**
* Select HSI as system clock source after Wake Up from Stop mode
*/
LL_RCC_SetClkAfterWakeFromStop(LL_RCC_STOP_WAKEUPCLOCK_HSI);
/* Initialize low power manager */
UTIL_LPM_Init();
/* Initialize the CPU2 reset value before starting CPU2 with C2BOOT */
LL_C2_PWR_SetPowerMode(LL_PWR_MODE_SHUTDOWN);
#if (CFG_USB_INTERFACE_ENABLE != 0)
/**
* Enable USB power
*/
HAL_PWREx_EnableVddUSB();
#endif /* CFG_USB_INTERFACE_ENABLE != 0 */
return;
}
static void appe_Tl_Init(void)
{
TL_MM_Config_t tl_mm_config;
SHCI_TL_HciInitConf_t SHci_Tl_Init_Conf;
/**< Reference table initialization */
TL_Init();
MtxShciId = osMutexNew(NULL);
SemShciId = osSemaphoreNew(1, 0, NULL); /*< Create the semaphore and make it busy at initialization */
/** FreeRTOS system task creation */
ShciUserEvtProcessId = osThreadNew(ShciUserEvtProcess, NULL, &ShciUserEvtProcess_attr);
/**< System channel initialization */
SHci_Tl_Init_Conf.p_cmdbuffer = (uint8_t*)&SystemCmdBuffer;
SHci_Tl_Init_Conf.StatusNotCallBack = APPE_SysStatusNot;
shci_init(APPE_SysUserEvtRx, (void*) &SHci_Tl_Init_Conf);
/**< Memory Manager channel initialization */
tl_mm_config.p_BleSpareEvtBuffer = BleSpareEvtBuffer;
tl_mm_config.p_SystemSpareEvtBuffer = SystemSpareEvtBuffer;
tl_mm_config.p_AsynchEvtPool = EvtPool;
tl_mm_config.AsynchEvtPoolSize = POOL_SIZE;
TL_MM_Init(&tl_mm_config);
TL_Enable();
return;
}
__WEAK void APP_BLE_Init(void)
{
}
static void APPE_SysStatusNot(SHCI_TL_CmdStatus_t status)
{
switch (status)
{
case SHCI_TL_CmdBusy:
osMutexAcquire(MtxShciId, osWaitForever);
break;
case SHCI_TL_CmdAvailable:
osMutexRelease(MtxShciId);
break;
default:
break;
}
return;
}
/**
* The type of the payload for a system user event is tSHCI_UserEvtRxParam
* When the system event is both :
* - a ready event (subevtcode = SHCI_SUB_EVT_CODE_READY)
* - reported by the FUS (sysevt_ready_rsp == FUS_FW_RUNNING)
* The buffer shall not be released
* (eg ((tSHCI_UserEvtRxParam*)pPayload)->status shall be set to SHCI_TL_UserEventFlow_Disable)
* When the status is not filled, the buffer is released by default
*/
static void APPE_SysUserEvtRx(void * pPayload)
{
TL_AsynchEvt_t *p_sys_event;
WirelessFwInfo_t WirelessInfo;
p_sys_event = (TL_AsynchEvt_t*)(((tSHCI_UserEvtRxParam*)pPayload)->pckt->evtserial.evt.payload);
switch(p_sys_event->subevtcode)
{
case SHCI_SUB_EVT_CODE_READY:
/* Read the firmware version of both the wireless firmware and the FUS */
SHCI_GetWirelessFwInfo(&WirelessInfo);
APP_DBG_MSG("Wireless Firmware version %d.%d.%d\n", WirelessInfo.VersionMajor, WirelessInfo.VersionMinor, WirelessInfo.VersionSub);
APP_DBG_MSG("Wireless Firmware build %d\n", WirelessInfo.VersionReleaseType);
APP_DBG_MSG("FUS version %d.%d.%d\n", WirelessInfo.FusVersionMajor, WirelessInfo.FusVersionMinor, WirelessInfo.FusVersionSub);
APP_DBG_MSG(">>== SHCI_SUB_EVT_CODE_READY\n\r");
APPE_SysEvtReadyProcessing(pPayload);
break;
case SHCI_SUB_EVT_ERROR_NOTIF:
APP_DBG_MSG(">>== SHCI_SUB_EVT_ERROR_NOTIF \n\r");
APPE_SysEvtError(pPayload);
break;
case SHCI_SUB_EVT_BLE_NVM_RAM_UPDATE:
APP_DBG_MSG(">>== SHCI_SUB_EVT_BLE_NVM_RAM_UPDATE -- BLE NVM RAM HAS BEEN UPDATED BY CPU2 \n");
APP_DBG_MSG(" - StartAddress = %lx , Size = %ld\n",
((SHCI_C2_BleNvmRamUpdate_Evt_t*)p_sys_event->payload)->StartAddress,
((SHCI_C2_BleNvmRamUpdate_Evt_t*)p_sys_event->payload)->Size);
break;
case SHCI_SUB_EVT_NVM_START_WRITE:
APP_DBG_MSG("==>> SHCI_SUB_EVT_NVM_START_WRITE : NumberOfWords = %ld\n",
((SHCI_C2_NvmStartWrite_Evt_t*)p_sys_event->payload)->NumberOfWords);
break;
case SHCI_SUB_EVT_NVM_END_WRITE:
APP_DBG_MSG(">>== SHCI_SUB_EVT_NVM_END_WRITE\n\r");
break;
case SHCI_SUB_EVT_NVM_START_ERASE:
APP_DBG_MSG("==>>SHCI_SUB_EVT_NVM_START_ERASE : NumberOfSectors = %ld\n",
((SHCI_C2_NvmStartErase_Evt_t*)p_sys_event->payload)->NumberOfSectors);
break;
case SHCI_SUB_EVT_NVM_END_ERASE:
APP_DBG_MSG(">>== SHCI_SUB_EVT_NVM_END_ERASE\n\r");
break;
default:
break;
}
return;
}
/**
* @brief Notify a system error coming from the M0 firmware
* @param ErrorCode : errorCode detected by the M0 firmware
*
* @retval None
*/
static void APPE_SysEvtError(void * pPayload)
{
TL_AsynchEvt_t *p_sys_event;
SCHI_SystemErrCode_t *p_sys_error_code;
p_sys_event = (TL_AsynchEvt_t*)(((tSHCI_UserEvtRxParam*)pPayload)->pckt->evtserial.evt.payload);
p_sys_error_code = (SCHI_SystemErrCode_t*) p_sys_event->payload;
APP_DBG_MSG(">>== SHCI_SUB_EVT_ERROR_NOTIF WITH REASON %x \n\r",(*p_sys_error_code));
if ((*p_sys_error_code) == ERR_BLE_INIT)
{
/* Error during BLE stack initialization */
APP_DBG_MSG(">>== SHCI_SUB_EVT_ERROR_NOTIF WITH REASON - ERR_BLE_INIT \n");
}
else
{
APP_DBG_MSG(">>== SHCI_SUB_EVT_ERROR_NOTIF WITH REASON - BLE ERROR \n");
}
return;
}
static void APPE_SysEvtReadyProcessing(void * pPayload)
{
TL_AsynchEvt_t *p_sys_event;
SHCI_C2_Ready_Evt_t *p_sys_ready_event;
SHCI_C2_CONFIG_Cmd_Param_t config_param = {0};
uint32_t RevisionID=0;
uint32_t DeviceID=0;
p_sys_event = (TL_AsynchEvt_t*)(((tSHCI_UserEvtRxParam*)pPayload)->pckt->evtserial.evt.payload);
p_sys_ready_event = (SHCI_C2_Ready_Evt_t*) p_sys_event->payload;
if (p_sys_ready_event->sysevt_ready_rsp == WIRELESS_FW_RUNNING)
{
/**
* The wireless firmware is running on the CPU2
*/
APP_DBG_MSG(">>== WIRELESS_FW_RUNNING \n");
/* Traces channel initialization */
APPD_EnableCPU2();
/* Enable all events Notification */
config_param.PayloadCmdSize = SHCI_C2_CONFIG_PAYLOAD_CMD_SIZE;
config_param.EvtMask1 = SHCI_C2_CONFIG_EVTMASK1_BIT0_ERROR_NOTIF_ENABLE
+ SHCI_C2_CONFIG_EVTMASK1_BIT1_BLE_NVM_RAM_UPDATE_ENABLE
+ SHCI_C2_CONFIG_EVTMASK1_BIT2_THREAD_NVM_RAM_UPDATE_ENABLE
+ SHCI_C2_CONFIG_EVTMASK1_BIT3_NVM_START_WRITE_ENABLE
+ SHCI_C2_CONFIG_EVTMASK1_BIT4_NVM_END_WRITE_ENABLE
+ SHCI_C2_CONFIG_EVTMASK1_BIT5_NVM_START_ERASE_ENABLE
+ SHCI_C2_CONFIG_EVTMASK1_BIT6_NVM_END_ERASE_ENABLE;
/* Read revision identifier */
/**
* @brief Return the device revision identifier
* @note This field indicates the revision of the device.
* @rmtoll DBGMCU_IDCODE REV_ID LL_DBGMCU_GetRevisionID
* @retval Values between Min_Data=0x00 and Max_Data=0xFFFF
*/
RevisionID = LL_DBGMCU_GetRevisionID();
APP_DBG_MSG(">>== DBGMCU_GetRevisionID= %lx \n\r", RevisionID);
config_param.RevisionID = (uint16_t)RevisionID;
DeviceID = LL_DBGMCU_GetDeviceID();
APP_DBG_MSG(">>== DBGMCU_GetDeviceID= %lx \n\r", DeviceID);
config_param.DeviceID = (uint16_t)DeviceID;
(void)SHCI_C2_Config(&config_param);
APP_BLE_Init();
UTIL_LPM_SetOffMode(1U << CFG_LPM_APP, UTIL_LPM_ENABLE);
}
else if (p_sys_ready_event->sysevt_ready_rsp == FUS_FW_RUNNING)
{
/**
* The FUS firmware is running on the CPU2
* In the scope of this application, there should be no case when we get here
*/
APP_DBG_MSG(">>== SHCI_SUB_EVT_CODE_READY - FUS_FW_RUNNING \n\r");
/* The packet shall not be released as this is not supported by the FUS */
((tSHCI_UserEvtRxParam*)pPayload)->status = SHCI_TL_UserEventFlow_Disable;
}
else
{
APP_DBG_MSG(">>== SHCI_SUB_EVT_CODE_READY - UNEXPECTED CASE \n\r");
}
return;
}
/*************************************************************
*
* FREERTOS WRAPPER FUNCTIONS
*
*************************************************************/
static void ShciUserEvtProcess(void *argument)
{
UNUSED(argument);
for(;;)
{
/* USER CODE BEGIN SHCI_USER_EVT_PROCESS_1 */
/* USER CODE END SHCI_USER_EVT_PROCESS_1 */
osThreadFlagsWait(1, osFlagsWaitAny, osWaitForever);
shci_user_evt_proc();
/* USER CODE BEGIN SHCI_USER_EVT_PROCESS_2 */
/* USER CODE END SHCI_USER_EVT_PROCESS_2 */
}
}
/* USER CODE BEGIN FD_LOCAL_FUNCTIONS */
/* USER CODE END FD_LOCAL_FUNCTIONS */
/*************************************************************
*
* WRAP FUNCTIONS
*
*************************************************************/
void HAL_Delay(uint32_t Delay)
{
uint32_t tickstart = HAL_GetTick();
uint32_t wait = Delay;
/* Add a freq to guarantee minimum wait */
if (wait < HAL_MAX_DELAY)
{
wait += HAL_GetTickFreq();
}
while ((HAL_GetTick() - tickstart) < wait)
{
/************************************************************************************
* ENTER SLEEP MODE
***********************************************************************************/
LL_LPM_EnableSleep(); /**< Clear SLEEPDEEP bit of Cortex System Control Register */
/**
* This option is used to ensure that store operations are completed
*/
#if defined (__CC_ARM) || defined (__ARMCC_VERSION)
__force_stores();
#endif /* __ARMCC_VERSION */
__WFI();
}
}
void shci_notify_asynch_evt(void* pdata)
{
UNUSED(pdata);
osThreadFlagsSet(ShciUserEvtProcessId, 1);
return;
}
void shci_cmd_resp_release(uint32_t flag)
{
UNUSED(flag);
osSemaphoreRelease(SemShciId);
return;
}
void shci_cmd_resp_wait(uint32_t timeout)
{
UNUSED(timeout);
osSemaphoreAcquire(SemShciId, osWaitForever);
return;
}
/* USER CODE BEGIN FD_WRAP_FUNCTIONS */
/* USER CODE END FD_WRAP_FUNCTIONS */
Core/Src/app_freertos.c
+58
View File
@@ -0,0 +1,58 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : app_freertos.c
* Description : Code for freertos applications
******************************************************************************
* @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 "FreeRTOS.h"
#include "task.h"
#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 ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
/* USER CODE END Variables */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE END Application */
Core/Src/bmm350.c
+2196
View File
File diff suppressed because it is too large Load Diff
Core/Src/bmm350.h
+656
View File
@@ -0,0 +1,656 @@
/**
* Copyright (c) 2025 Bosch Sensortec GmbH. All rights reserved.
*
* BSD-3-Clause
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* @file bmm350.h
* @date 2025-10-30
* @version v1.10.0
*
*/
/*!
* @defgroup bmm350 BMM350
* @brief Sensor driver for BMM350 sensor
*/
#ifndef _BMM350_H
#define _BMM350_H
/*! CPP guard */
#ifdef __cplusplus
extern "C" {
#endif
/*************************** Header files *******************************/
#include "bmm350_defs.h"
/******************* Function prototype declarations ********************/
/**
* \ingroup bmm350
* \defgroup bmm350ApiVersion Information
* @brief Get the BMM350 SensorAPI Version
*/
/*!
* \ingroup bmm350ApiVersion
* \page bmm350_api_bmm350_api_version bmm350_api_version
* \code
* int8_t bmm350_api_version(struct bmm350_version *api_verison);
* \endcode
* @details This API gives the release version details of the BMM350 SensorAPI.
*
* @param[out] version : Structure instance of bmm350_version
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_api_version(struct bmm350_version *api_version);
/**
* \ingroup bmm350
* \defgroup bmm350ApiInit Initialization
* @brief Initialize the sensor and device structure
*/
/*!
* \ingroup bmm350ApiInit
* \page bmm350_api_bmm350_init bmm350_init
* \code
* int8_t bmm350_init(struct bmm350_dev *dev);
* \endcode
* @details This API is the entry point. Call this API before using other APIs.
* This API reads the chip-id of the sensor which is the first step to
* verify the sensor and also it configures the read mechanism of I2C and
* I3C interface.
*
* @param[in,out] dev : Structure instance of bmm350_dev
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_init(struct bmm350_dev *dev);
/**
* \ingroup bmm350
* \defgroup bmm350ApiReset Reset
* @brief Reset APIs
*/
/*!
* \ingroup bmm350ApiReset
* \page bmm350_api_bmm350_soft_reset bmm350_soft_reset
* \code
* int8_t bmm350_soft_reset(struct bmm350_dev *dev);
* \endcode
* @details This API is used to perform soft-reset of the sensor
* where all the registers are reset to their default values.
*
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_soft_reset(struct bmm350_dev *dev);
/**
* \ingroup bmm350
* \defgroup bmm350ApiSetGet Set-Get
* @brief Set and Get APIs
*/
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_regs bmm350_set_regs
* \code
* int8_t bmm350_set_regs(uint8_t reg_addr, const uint8_t *reg_data, uint16_t len, struct bmm350_dev *dev);
* \endcode
* @details This API writes the given data to the register address
* of the sensor.
*
* @param[in] reg_addr : Register address from where the data to be written.
* @param[in] reg_data : Pointer to data buffer which is to be written
* in the reg_addr of sensor.
* @param[in] len : No of bytes of data to write..
* @param[in, out] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_regs(uint8_t reg_addr, const uint8_t *reg_data, uint16_t len, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_get_regs bmm350_get_regs
* \code
* int8_t bmm350_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint16_t len, struct bmm350_dev *dev);
* \endcode
* @details This API reads the data from the given register address of sensor.
*
* @param[in] reg_addr : Register address from where the data to be read
* @param[out] reg_data : Pointer to data buffer to store the read data.
* @param[in] len : No of bytes of data to be read.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_get_regs(uint8_t reg_addr, uint8_t *reg_data, uint16_t len, struct bmm350_dev *dev);
/**
* \ingroup bmm350
* \defgroup bmm350ApiDelay Delay
* @brief Delay function in microseconds
*/
/*!
* \ingroup bmm350ApiDelay
* \page bmm350_api_bmm350_delay_us bmm350_delay_us
* \code
* int8_t bmm350_delay_us(uint32_t period_us, const struct bmm350_dev *dev);
* \endcode
* @details This function provides the delay for required time (Microsecond) as per the input provided in some of the
* APIs.
*
* @param[in] period_us : The required wait time in microsecond.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_delay_us(uint32_t period_us, const struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_get_interrupt_status bmm350_get_interrupt_status
* \code
* int8_t bmm350_get_interrupt_status(uint8_t *drdy_status, struct bmm350_dev *dev);
* \endcode
* @details This API obtains the status flags of all interrupt
* which is used to check for the assertion of interrupts
*
* @param[in,out] drdy_status : Variable to store data ready interrupt status.
* @param[in,out] dev : Structure instance of bmm350_dev.
*
*
* @return Result of API execution status and self test result.
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_get_interrupt_status(uint8_t *drdy_status, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_powermode bmm350_set_powermode
* \code
* int8_t bmm350_set_powermode(enum bmm350_power_modes powermode, struct bmm350_dev *dev);
* \endcode
* @details This API is used to set the power mode of the sensor
*
* @param[in] powermode : Set power mode
* @param[in] dev : Structure instance of bmm350_dev.
*
*@verbatim
powermode | Power mode
-------------------------|-----------------------
| BMM350_SUSPEND_MODE
| BMM350_NORMAL_MODE
| BMM350_FORCED_MODE
| BMM350_FORCED_MODE_FAST
*@endverbatim
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_powermode(enum bmm350_power_modes powermode, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_odr_performance bmm350_set_odr_performance
* \code
* int8_t bmm350_set_odr_performance(enum bmm350_data_rates odr,
* enum bmm350_performance_parameters avg,
* struct bmm350_dev *dev);
*
* \endcode
* @details This API sets the ODR and averaging factor.
* If ODR and performance is a combination which is not allowed, then
* the combination setting is corrected to the next lower possible setting
*
* @param[in] odr : enum bmm350_data_rates
*
*@verbatim
Data rate (ODR) | odr
-------------------------|-----------------------
400Hz | BMM350_DATA_RATE_400HZ
200Hz | BMM350_DATA_RATE_200HZ
100Hz | BMM350_DATA_RATE_100HZ
50Hz | BMM350_DATA_RATE_50HZ
25Hz | BMM350_DATA_RATE_25HZ
12.5Hz | BMM350_DATA_RATE_12_5HZ
6.25Hz | BMM350_DATA_RATE_6_25HZ
3.125Hz | BMM350_DATA_RATE_3_125HZ
1.5625Hz | BMM350_DATA_RATE_1_5625HZ
*@endverbatim
*
* @param[in] avg : enum bmm350_performance_parameters
*
*@verbatim
avg | averaging factor alias
---------------------------|------------------------------------------
low power/highest noise | BMM350_NO_AVERAGING BMM350_LOWPOWER
lesser noise | BMM350_AVERAGING_2 BMM350_REGULARPOWER
even lesser noise | BMM350_AVERAGING_4 BMM350_LOWNOISE
lowest noise/highest power | BMM350_AVERAGING_8 BMM350_ULTRALOWNOISE
*@endverbatim
*
* @param[in,out] dev : Structure instance of bmm350_dev
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_odr_performance(enum bmm350_data_rates odr,
enum bmm350_performance_parameters avg,
struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_enable_axes bmm350_enable_axes
* \code
* int8_t bmm350_enable_axes(enum bmm350_x_axis_en_dis en_x, enum bmm350_y_axis_en_dis en_y, enum bmm350_z_axis_en_dis en_z, struct bmm350_dev *dev);
* \endcode
* @details This API is used to enable or disable the magnetic
* measurement of x,y,z axes
*
* @param[in] en_x : Enable or disable X axis
* @param[in] en_y : Enable or disable Y axis
* @param[in] en_z : Enable or disable Z axis
* @param[in,out] dev : Structure instance of bmm350_dev.
*
*@verbatim
Value | Axis (en_x, en_y, en_z)
-------------------|-----------------------
BMM350_ENABLE | Enabled
BMM350_DISABLE | Disabled
*@endverbatim
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_enable_axes(enum bmm350_x_axis_en_dis en_x,
enum bmm350_y_axis_en_dis en_y,
enum bmm350_z_axis_en_dis en_z,
struct bmm350_dev *dev);
/**
* \ingroup bmm350
* \defgroup bmm350ApiRead Sensortime
* @brief Reads sensortime
*/
/*!
* \ingroup bmm350ApiRead
* \page bmm350_api_bmm350_read_sensortime bmm350_read_sensortime
* \code
* int8_t bmm350_read_sensortime(uint32_t *seconds, uint32_t *nanoseconds, struct bmm350_dev *dev);
* \endcode
* @details This API is used to read the sensor time.
* It converts the sensor time register values to the representative time value.
* Returns the sensor time in ticks.
*
* @param[out] seconds : Variable to get sensor time in seconds.
* @param[out] nanoseconds : Variable to get sensor time in nanoseconds.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_read_sensortime(uint32_t *seconds, uint32_t *nanoseconds, struct bmm350_dev *dev);
/**
* \ingroup bmm350
* \defgroup bmm350ApiInterrupt Enable Interrupt
* @brief Interrupt enable APIs
*/
/*!
* \ingroup bmm350ApiInterrupt
* \page bmm350_api_bmm350_enable_interrupt bmm350_enable_interrupt
* \code
* int8_t bmm350_enable_interrupt(enum bmm350_interrupt_enable_disable enable_disable, struct bmm350_dev *dev);
* \endcode
* @details This API is used to enable or disable the data ready interrupt
*
* @param[in] enable_disable : Enable/ Disable data ready interrupt.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_enable_interrupt(enum bmm350_interrupt_enable_disable enable_disable, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiInterrupt
* \page bmm350_api_bmm350_configure_interrupt bmm350_configure_interrupt
* \code
* int8_t bmm350_configure_interrupt(enum bmm350_intr_latch latching,
* enum bmm350_intr_polarity polarity,
* enum bmm350_intr_drive drivertype,
* enum bmm350_intr_map map_nomap,
* struct bmm350_dev *dev);
* \endcode
* @details This API is used to configure the interrupt control settings.
*
* @param[in] latching : Sets either latched or pulsed.
* @param[in] polarity : Sets either polarity high or low.
* @param[in] drivertype : Sets either open drain or push pull.
* @param[in] map_nomap : Sets either map or unmap the pins.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_configure_interrupt(enum bmm350_intr_latch latching,
enum bmm350_intr_polarity polarity,
enum bmm350_intr_drive drivertype,
enum bmm350_intr_map map_nomap,
struct bmm350_dev *dev);
/**
* \ingroup bmm350
* \defgroup bmm350ApiUncompMag Uncompensated mag
* @brief Reads uncompensated mag and temperature data
*/
/*!
* \ingroup bmm350ApiUncompMag
* \page bmm350_api_bmm350_read_uncomp_mag_temp_data bmm350_read_uncomp_mag_temp_data
* \code
* int8_t bmm350_read_uncomp_mag_temp_data(struct bmm350_raw_mag_data *raw_data, struct bmm350_dev *dev);
* \endcode
* @details This API is used to read uncompensated mag and temperature data
*
* @param[in, out] raw_data : Structure instance of bmm350_raw_mag_data.
* @param[in, out] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_read_uncomp_mag_temp_data(struct bmm350_raw_mag_data *raw_data, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_int_ctrl_ibi bmm350_set_int_ctrl_ibi
* \code
* int8_t bmm350_set_int_ctrl_ibi(enum bmm350_drdy_int_map_to_ibi en_dis,
* enum bmm350_clear_drdy_int_status_upon_ibi clear_on_ibi, struct bmm350_dev *dev);
* \endcode
* @details This API sets the interrupt control IBI configurations to the sensor.
* And enables conventional interrupt if IBI is enabled.
*
* @param[in] en_dis : Sets either enable or disable IBI.
* @param[in] clear_on_ibi : sets either clear or no clear on IBI.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_int_ctrl_ibi(enum bmm350_drdy_int_map_to_ibi en_dis,
enum bmm350_clear_drdy_int_status_upon_ibi clear_on_ibi,
struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_pad_drive bmm350_set_pad_drive
* \code
* int8_t bmm350_set_pad_drive(uint8_t drive, struct bmm350_dev *dev);
* \endcode
* @details This API is used to set the pad drive strength
*
* @param[in] drive : Drive settings, range 0 (weak) ..7(strong)
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_pad_drive(uint8_t drive, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiReset
* \page bmm350_api_bmm350_magnetic_reset_and_wait bmm350_magnetic_reset_and_wait
* \code
* int8_t bmm350_magnetic_reset_and_wait(struct bmm350_dev *dev)
* \endcode
* @details This API is used to perform the magnetic reset of the sensor
* which is necessary after a field shock (400mT field applied to sensor).
* It sends flux guide or bit reset to the device in suspend mode.
*
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_magnetic_reset_and_wait(struct bmm350_dev *dev);
#ifdef BMM350_USE_FIXED_POINT
/**
* \ingroup bmm350ApiMagTemp
* \page bmm350_api_bmm350_get_compensated_mag_xyz_temp_data_fixed bmm350_get_compensated_mag_xyz_temp_data_fixed
* \code
* int8_t bmm350_get_compensated_mag_xyz_temp_data_fixed(struct bmm350_mag_temp_data *mag_temp_data, struct bmm350_dev *dev);
* \endcode
* @details This API is used to read mag and temperature data in the units of uT and *C
*
* @param[in, out] mag_temp_data : Structure instance of bmm350_mag_temp_data.
* @param[in, out] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_get_compensated_mag_xyz_temp_data_fixed(struct bmm350_mag_temp_data *mag_temp_data,
struct bmm350_dev *dev);
#else
/**
* \ingroup bmm350
* \defgroup bmm350ApiMagComp Compensation
* @brief Compensation for mag x,y,z axis and temperature API
*/
/*!
* \ingroup bmm350ApiMagComp
* \page bmm350_api_bmm350_get_compensated_mag_xyz_temp_data bmm350_get_compensated_mag_xyz_temp_data
* \code
* int8_t bmm350_get_compensated_mag_xyz_temp_data(struct bmm350_mag_temp_data *mag_temp_data, struct bmm350_dev *dev);
* \endcode
* @details This API is used to perform compensation for raw magnetometer and temperature data.
*
* @param[out] mag_temp_data : Structure instance of bmm350_mag_temp_data.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_get_compensated_mag_xyz_temp_data(struct bmm350_mag_temp_data *mag_temp_data, struct bmm350_dev *dev);
#endif
/**
* \ingroup bmm350
* \defgroup bmm350ApiSelftest Self-test
* @brief Perform self-test for x and y axis
*/
/*!
* \ingroup bmm350ApiSelftest
* \page bmm350_api_bmm350_perform_self_test bmm350_perform_self_test
* \code
* int8_t bmm350_perform_self_test(struct bmm350_self_test *out_data, struct bmm350_dev *dev);
* \endcode
* @details This API executes FGR and BR sequences to initialize TMR sensor and performs self-test for x and y axis.
*
* @param[in, out] out_data : Structure instance of bmm350_self_test.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_perform_self_test(struct bmm350_self_test *out_data, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_i2c_wdt bmm350_set_i2c_wdt
* \code
* int8_t bmm350_set_i2c_wdt(enum bmm350_i2c_wdt_en i2c_wdt_en_dis, enum bmm350_i2c_wdt_sel i2c_wdt_sel,
* struct bmm350_dev *dev);
* \endcode
* @details This API sets the I2C watchdog timer configurations to the sensor.
*
* @param[in] i2c_wdt_en_dis : Enable/ Disable I2C watchdog timer.
* @param[in] i2c_wdt_sel : I2C watchdog timer selection period.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_i2c_wdt(enum bmm350_i2c_wdt_en i2c_wdt_en_dis,
enum bmm350_i2c_wdt_sel i2c_wdt_sel,
struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_tmr_selftest_user bmm350_set_tmr_selftest_user
* \code
* int8_t bmm350_set_tmr_selftest_user(enum bmm350_st_igen_en st_igen_en_dis,
* enum bmm350_st_n st_n_en_dis,
* enum bmm350_st_p st_p_en_dis,
* enum bmm350_ist_en_x ist_x_en_dis,
* enum bmm350_ist_en_y ist_y_en_dis,
* struct bmm350_dev *dev);
* \endcode
* @details This API sets the TMR user self-test register
*
* @param[in] st_igen_en_dis : Enable/ Disable self-test internal current gen.
* @param[in] st_n_en_dis : Enable/ Disable dc_st_n signal.
* @param[in] st_p_en_dis : Enable/ Disable dc_st_p signal.
* @param[in] ist_x_en_dis : Enable/ Disable dc_ist_en_x signal.
* @param[in] ist_y_en_dis : Enable/ Disable dc_ist_en_y signal.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_tmr_selftest_user(enum bmm350_st_igen_en st_igen_en_dis,
enum bmm350_st_n st_n_en_dis,
enum bmm350_st_p st_p_en_dis,
enum bmm350_ist_en_x ist_x_en_dis,
enum bmm350_ist_en_y ist_y_en_dis,
struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_set_ctrl_user bmm350_set_ctrl_user
* \code
* int8_t bmm350_set_ctrl_user(enum bmm350_ctrl_user cfg_sens_tim_aon_en_dis, struct bmm350_dev *dev);
* \endcode
* @details This API sets the control user configurations to the sensor.
*
* @param[in] cfg_sens_tim_aon_en_dis : Enable/ Disable configuration of sensor time to run on suspend mode.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_set_ctrl_user(enum bmm350_ctrl_user cfg_sens_tim_aon_en_dis, struct bmm350_dev *dev);
/*!
* \ingroup bmm350ApiSetGet
* \page bmm350_api_bmm350_get_pmu_cmd_status_0 bmm350_get_pmu_cmd_status_0
* \code
* int8_t bmm350_get_pmu_cmd_status_0(struct bmm350_pmu_cmd_status_0 *pmu_cmd_stat_0, struct bmm350_dev *dev);
* \endcode
* @details This API gets the PMU command status 0 value.
*
* @param[out] pmu_cmd_stat_0 : Structure instance of bmm350_pmu_cmd_status_0.
* @param[in] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_get_pmu_cmd_status_0(struct bmm350_pmu_cmd_status_0 *pmu_cmd_stat_0, struct bmm350_dev *dev);
/*!
* @brief This internal API is used to compute the square root in fixed point.
* @param[in] inp : input, whose square root needs to be computed
*
* @return square root of the input
*/
uint16_t bmm350_fixed_point_sqrt(uint32_t inp);
#ifdef BMM350_USE_FIXED_POINT
fixed_t fixed_add(fixed_t a, fixed_t b);
fixed_t fixed_sub(fixed_t a, fixed_t b);
fixed_t fixed_mul_A48_16(fixed_t a, fixed_t b);
fixed_t fixed_div(fixed_t a, fixed_t b);
#endif
#ifdef __cplusplus
}
#endif /* End of CPP guard */
#endif /* _BMM350_H */
Core/Src/bmm350_defs.h
+1121
View File
File diff suppressed because it is too large Load Diff
Core/Src/bmm350_oor.c
+540
View File
@@ -0,0 +1,540 @@
/**
* Copyright (c) 2025 Bosch Sensortec GmbH. All rights reserved.
*
* BSD-3-Clause
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* @file bmm350_oor.c
* @date 2025-10-30
* @version v1.10.0
*
*/
#include "bmm350.h"
#include "bmm350_oor.h"
#ifndef BMM350_USE_FIXED_POINT
#include "math.h"
#else
#ifndef __KERNEL__
#include "stdlib.h"
#endif
#endif
/********************** Static function declarations ************************/
/*!
* @brief This internal API is used to execute delay operation for the reset functions.
*/
static int8_t execute_delay_in_steps(uint32_t delay, uint32_t delay_step, const struct bmm350_dev *dev);
#ifdef BMM350_OOR_HALF_SELF_TEST
/*!
* @brief This internal API is used to trigger half self-test
* @param[out] oor : Structure that stores the state of the out of range detector
* @param[in,out] dev : Device structure of the BMM350
*/
static int8_t trigger_half_selftest(const struct bmm350_oor_reset_delay *rdelay,
struct bmm350_oor_params *oor,
struct bmm350_dev *dev)
{
int8_t rslt = BMM350_OK;
uint8_t pmu_cmd = BMM350_PMU_CMD_BR;
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
/* Trigger a self-test on every alternate measurement if needed */
if (oor->enable_selftest)
{
oor->st_counter++;
switch (oor->st_counter)
{
case 1:
oor->st_cmd = BMM350_SELF_TEST_POS_X;
break;
case 2:
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
break;
case 3:
oor->st_cmd = BMM350_SELF_TEST_POS_Y;
break;
case 4:
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
break;
case 5:
if (dev->enable_auto_br == BMM350_DISABLE)
{
/* Trigger the Bit Reset */
pmu_cmd = BMM350_PMU_CMD_BR;
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
if (rslt == BMM350_OK)
{
/* Bit Reset delay*/
(void)execute_delay_in_steps(rdelay->br_delay, rdelay->delay_step, dev);
}
}
else
{
oor->st_counter = 0;
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
}
break;
default:
oor->st_counter = 0;
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
break;
}
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
}
else
{
if (oor->last_st_cmd != BMM350_SELF_TEST_DISABLE)
{
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
oor->st_counter = 0;
}
}
return rslt;
}
/*!
* @brief This internal API is used to validate half self-test
* @param[out] data : Sensor data
* @param[out] oor : Structure that stores the state of the out of range detector
*/
static void validate_half_selftest(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor)
{
switch (oor->last_st_cmd)
{
case BMM350_SELF_TEST_DISABLE:
oor->mag_x_st_dis = data->x;
oor->mag_y_st_dis = data->y;
break;
case BMM350_SELF_TEST_POS_X:
oor->mag_x_st_en = data->x;
oor->x_failed = (oor->mag_x_st_en - oor->mag_x_st_dis) < BMM350_HALF_ST_THRESHOLD ? true : false;
break;
case BMM350_SELF_TEST_POS_Y:
oor->mag_y_st_en = data->y;
oor->y_failed = (oor->mag_y_st_en - oor->mag_y_st_dis) < BMM350_HALF_ST_THRESHOLD ? true : false;
break;
default:
break;
}
}
#else
/*!
* @brief This internal API is used to trigger self-test
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
* @param[out] oor : Structure that stores the state of the out of range detector
* @param[in,out] dev : Device structure of the BMM350
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
static int8_t trigger_selftest(const struct bmm350_oor_reset_delay *rdelay,
struct bmm350_oor_params *oor,
struct bmm350_dev *dev)
{
int8_t rslt = BMM350_OK;
uint8_t pmu_cmd = BMM350_PMU_CMD_BR;
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
/* Trigger a self-test on every alternate measurement if needed */
if (oor->enable_selftest)
{
oor->st_counter++;
switch (oor->st_counter)
{
case 1:
oor->st_cmd = BMM350_SELF_TEST_POS_X;
break;
case 2:
oor->st_cmd = BMM350_SELF_TEST_NEG_X;
break;
case 3:
oor->st_cmd = BMM350_SELF_TEST_POS_Y;
break;
case 4:
oor->st_cmd = BMM350_SELF_TEST_NEG_Y;
break;
case 5:
if (dev->enable_auto_br == BMM350_DISABLE)
{
/* Trigger the Bit Reset */
pmu_cmd = BMM350_PMU_CMD_BR;
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
if (rslt == BMM350_OK)
{
/* Bit Reset delay*/
(void)execute_delay_in_steps(rdelay->br_delay, rdelay->delay_step, dev);
}
}
else
{
oor->st_counter = 0;
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
}
break;
default:
oor->st_counter = 0;
oor->st_cmd = BMM350_SELF_TEST_DISABLE;
break;
}
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
}
else
{
if (oor->last_st_cmd != BMM350_SELF_TEST_DISABLE)
{
rslt = bmm350_set_regs(BMM350_REG_TMR_SELFTEST_USER, &(oor->st_cmd), 1, dev);
oor->st_counter = 0;
}
}
return rslt;
}
/*!
* @brief This internal API is used to validate self-test during the self-test window
* @param[in] data : Sensor data
* @param[out] oor : Structure that stores the state of the out of range detector
*/
static void validate_selftest_window(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor)
{
switch (oor->last_st_cmd)
{
case BMM350_SELF_TEST_POS_X:
oor->mag_x_st_en = data->x;
break;
case BMM350_SELF_TEST_NEG_X:
oor->mag_x_st_dis = data->x;
oor->x_failed = (oor->mag_x_st_en - oor->mag_x_st_dis) < BMM350_FULL_ST_THRESHOLD ? true : false;
break;
case BMM350_SELF_TEST_POS_Y:
oor->mag_y_st_en = data->y;
break;
case BMM350_SELF_TEST_NEG_Y:
oor->mag_y_st_dis = data->y;
oor->y_failed = (oor->mag_y_st_en - oor->mag_y_st_dis) < BMM350_FULL_ST_THRESHOLD ? true : false;
break;
default:
break;
}
}
#endif
/********************** Global function definitions ************************/
/*!
* @brief This internal API is used to validate out of range.
* @param[in] data : Sensor data
* @param[out] oor : Structure that stores the state of the out of range detector
*/
void bmm350_oor_validate_out_of_range(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor)
{
#ifdef BMM350_USE_FIXED_POINT
/* Threshold to start out of range detection */
fixed_t threshold = BMM350_OUT_OF_RANGE_THRESHOLD;
/* Threshold to start self-tests */
fixed_t st_threshold = BMM350_SELF_TEST_THRESHOLD;
/* Variable to compute the magnitude square */
fixed_t magnitude_square = 0;
#else
/* Threshold to start out of range detection */
float threshold = BMM350_OUT_OF_RANGE_THRESHOLD;
/* Threshold to start self-tests */
float st_threshold = BMM350_SELF_TEST_THRESHOLD;
#endif
#ifdef BMM350_USE_FIXED_POINT
/* Compute the Field Strength */
magnitude_square =
(uint32_t)((fixed_mul_A48_16(data->x,
data->x) +
fixed_mul_A48_16(data->y, data->y) + fixed_mul_A48_16(data->z, data->z)) >> F16_FRAC_BITS);
oor->field_strength = (uint32_t)bmm350_fixed_point_sqrt(magnitude_square);
#else
/* Compute the Field Strength */
oor->field_strength = sqrtf((data->x * data->x) + (data->y * data->y) + (data->z * data->z));
#endif
/* If either self-test failed, alert that the sensor is out of range and continue self-tests */
if (oor->x_failed || oor->y_failed)
{
oor->out_of_range = true;
oor->enable_selftest = true;
}
else
{
/* Check for the self-test threshold and perform self-tests to catch if the sensor is out of range */
#ifdef BMM350_USE_FIXED_POINT
if ((abs(data->x) >= st_threshold) || (abs(data->y) >= st_threshold) || (abs(data->z) >= st_threshold) ||
(oor->field_strength >= (uint32_t)(st_threshold >> F16_FRAC_BITS)))
#else
if ((fabsf(data->x) >= st_threshold) || (fabsf(data->y) >= st_threshold) || (fabsf(data->z) >= st_threshold) ||
(oor->field_strength >= st_threshold))
#endif
{
oor->enable_selftest = true;
}
else if (oor->st_counter == 0) /* If a self-test procedure has started, wait for it to complete */
{
oor->enable_selftest = false;
}
/* If out of range was previously detected, reduce the threshold to get back in range,
* effectively preventing hysteresis. Selecting 400uT */
if (oor->out_of_range)
{
threshold = BMM350_IN_RANGE_THRESHOLD;
}
/* Check if X or Y or Z > the threshold or the magnitude of all 3 is greater */
#ifdef BMM350_USE_FIXED_POINT
if ((abs(data->x) >= threshold) || (abs(data->y) >= threshold) || (abs(data->z) >= threshold) ||
(oor->field_strength >= (uint32_t)(threshold >> F16_FRAC_BITS)))
#else
if ((fabsf(data->x) >= threshold) || (fabsf(data->y) >= threshold) || (fabsf(data->z) >= threshold) ||
(oor->field_strength >= threshold))
#endif
{
oor->out_of_range = true;
}
else if (oor->st_counter == 0) /* If a self-test procedure has started, wait for it to complete */
{
if (oor->out_of_range)
{
oor->trigger_reset = true;
}
oor->out_of_range = false;
}
}
}
/*!
* @brief This API is used to perform magnetic reset sequence.
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
* @param[out] oor : Structure that stores the state of the out of range detector
* @param[in,out] dev : Device structure of the BMM350
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_oor_perform_reset_sequence(const struct bmm350_oor_reset_delay *rdelay,
struct bmm350_oor_params *oor,
struct bmm350_dev *dev)
{
int8_t rslt = 0;
uint8_t pmu_cmd = 0;
oor->reset_counter++;
switch (oor->reset_counter)
{
case 1: /* Trigger the Bit reset fast */
pmu_cmd = BMM350_PMU_CMD_BR_FAST;
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
if (rslt == BMM350_OK)
{
/* Bit Reset Window synchronization delay*/
(void)execute_delay_in_steps(rdelay->br_delay, rdelay->delay_step, dev);
}
break;
case 2: /* Trigger Flux Guide reset */
pmu_cmd = BMM350_PMU_CMD_FGR;
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
if (rslt == BMM350_OK)
{
/* Flux Guide Reset Window synchronization delay*/
(void)execute_delay_in_steps(rdelay->fgr_delay, rdelay->delay_step, dev);
}
break;
case 3: /* Flux Guide dummy */
break;
default: /* Default acts like the Flux guide reset dummy */
oor->reset_counter = 0;
oor->trigger_reset = false;
break;
}
return rslt;
}
/*!
* @brief This API is used to read out of range in during self-test.
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
* @param[out] data : Sensor data
* @param[out] oor : Structure that stores the state of the out of range detector
* @param[in,out] dev : Device structure of the BMM350
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_oor_read(const struct bmm350_oor_reset_delay *rdelay,
struct bmm350_mag_temp_data *data,
struct bmm350_oor_params *oor,
struct bmm350_dev *dev)
{
int8_t rslt = 0;
uint8_t pmu_cmd = BMM350_PMU_CMD_SUS;
#ifdef BMM350_OOR_HALF_SELF_TEST
rslt = trigger_half_selftest(rdelay, oor, dev);
#else
rslt = trigger_selftest(rdelay, oor, dev);
#endif
if (rslt == BMM350_OK)
{
pmu_cmd = BMM350_PMU_CMD_FM_FAST;
rslt = bmm350_set_regs(BMM350_REG_PMU_CMD, &pmu_cmd, 1, dev);
if (rslt == BMM350_OK)
{
#ifdef BMM350_USE_FIXED_POINT
rslt = bmm350_get_compensated_mag_xyz_temp_data_fixed(data, dev);
#else
rslt = bmm350_get_compensated_mag_xyz_temp_data(data, dev);
#endif
}
}
#ifdef BMM350_OOR_HALF_SELF_TEST
validate_half_selftest(data, oor);
#else
validate_selftest_window(data, oor);
#endif
bmm350_oor_validate_out_of_range(data, oor);
oor->last_st_cmd = oor->st_cmd;
return rslt;
}
/*!
* @brief This API is used to computing the reset delay settings for Magnetic Reset Sequence.
* @param[in] odr_config : ODR Configuration
* @param[out] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
*/
void bmm350_oor_compute_delay_setting(uint8_t odr_config, struct bmm350_oor_reset_delay *rdelay)
{
uint16_t min_dealy_unit = 2500; /* corresponds to 400Hz -> (1/400) * (10^6) us */
#ifdef BMM350_USE_FIXED_POINT
uint32_t power_scale = ((uint32_t)1 << (odr_config - BMM350_ODR_400HZ));
rdelay->delay_step = (uint32_t)(min_dealy_unit * power_scale);
#else
rdelay->delay_step = (uint32_t)(min_dealy_unit * (pow(2.0, (double)(odr_config - BMM350_ODR_400HZ))));
#endif
rdelay->br_delay = ((rdelay->delay_step * 3) - 1000);
rdelay->fgr_delay = ((rdelay->delay_step * 4) - 2000);
}
/********************** Static function definitions ************************/
/*!
* @brief This internal API is used to execute delay operation for the reset functions.
* @param[in] delay : ODR Window Delay
* @param[in] delay_step : Delay Time Resolution
* @param[in,out] dev : Device structure of the BMM350
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
static int8_t execute_delay_in_steps(uint32_t delay, uint32_t delay_step, const struct bmm350_dev *dev)
{
int8_t rslt = BMM350_OK;
if ((delay > 0) && (delay_step > 0) && (delay >= delay_step))
{
while (delay >= delay_step)
{
rslt = bmm350_delay_us(delay_step, dev);
if (rslt == BMM350_OK)
{
delay = delay - delay_step;
}
else
{
break;
}
}
}
else
{
rslt = BMM350_E_INVALID_INPUT;
}
return rslt;
}
Core/Src/bmm350_oor.h
+202
View File
@@ -0,0 +1,202 @@
/**
* Copyright (c) 2025 Bosch Sensortec GmbH. All rights reserved.
*
* BSD-3-Clause
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* @file bmm350_oor.h
* @date 2025-10-30
* @version v1.10.0
*
*/
#ifndef _BMM350_OOR_H
#define _BMM350_OOR_H
#ifdef __KERNEL__
#include <linux/types.h>
#include <linux/kernel.h>
#else
#include <stdbool.h>
#ifdef BMM350_USE_FIXED_POINT
#include <math.h>
#endif
#endif
#include "bmm350.h"
/*! CPP guard */
#ifdef __cplusplus
extern "C" {
#endif
/******************************************************************************/
/*! @name General Macro Definitions */
/******************************************************************************/
/*! Macro to define half self-test for out of range
* NOTE: Comment this to use generic self test */
/*#define BMM350_OOR_HALF_SELF_TEST */
/*! Macro to synchronize the reset window with the measurement window */
#define BMM350_OOR_WINDOW_SYNC_DELAY UINT16_C(9000)
/*! Macro to define mag data minimum and maximum range in uT */
#ifdef BMM350_USE_FIXED_POINT
#define BMM350_HALF_ST_THRESHOLD (130 << F16_FRAC_BITS)
#define BMM350_FULL_ST_THRESHOLD (300 << F16_FRAC_BITS)
#else
#define BMM350_HALF_ST_THRESHOLD (130.0f)
#define BMM350_FULL_ST_THRESHOLD (300.0f)
#endif
/*! Macro to define threshold values of in range, out of range and self-test */
#ifdef BMM350_USE_FIXED_POINT
#define BMM350_IN_RANGE_THRESHOLD (2000 << F16_FRAC_BITS)
#define BMM350_OUT_OF_RANGE_THRESHOLD (2400 << F16_FRAC_BITS)
#define BMM350_SELF_TEST_THRESHOLD (2600 << F16_FRAC_BITS)
#else
#define BMM350_IN_RANGE_THRESHOLD (2000.0f)
#define BMM350_OUT_OF_RANGE_THRESHOLD (2400.0f)
#define BMM350_SELF_TEST_THRESHOLD (2600.0f)
#endif
/************************* Structure definitions *************************/
/*!
* @brief Structure to define bmm350 out of range parameters
*/
struct bmm350_oor_params
{
/*! Field Strength */
#ifdef BMM350_USE_FIXED_POINT
uint32_t field_strength;
#else
float field_strength;
#endif
/*! Flags to enable Out of Range */
bool out_of_range;
/*! Counter to track what self test to trigger */
uint8_t st_counter;
/*! Current self-test command */
uint8_t st_cmd;
/*! Stores the last applied self test configuration */
uint8_t last_st_cmd;
/*! Self test enabled/disabled measurements for X and Y */
#ifdef BMM350_USE_FIXED_POINT
fixed_t mag_x_st_en, mag_x_st_dis, mag_y_st_en, mag_y_st_dis, mag_x_st_dis_avg, mag_y_st_dis_avg;
#else
float mag_x_st_en, mag_x_st_dis, mag_y_st_en, mag_y_st_dis, mag_x_st_dis_avg, mag_y_st_dis_avg;
#endif
/*! Flags to track if the test failed to redo it */
bool x_failed, y_failed;
/*! Flags to enable self-test */
bool enable_selftest;
/*! Flags to trigger reset */
bool trigger_reset;
/*! Variable to store reset counter value */
uint8_t reset_counter;
};
/*!
* @brief Structure to define bmm350 out of range reset delay parameters
*/
struct bmm350_oor_reset_delay
{
uint32_t delay_step;
uint32_t br_delay;
uint32_t fgr_delay;
};
/******************* Function prototype declarations ********************/
/*!
* @brief Function to read data and validate if the sensor is out of range
*
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
* @param[out] data : Sensor data
* @param[out] oor : Structure that stores the state of the out of range detector
* @param[in,out] dev : Device structure of the BMM350
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_oor_read(const struct bmm350_oor_reset_delay *rdelay,
struct bmm350_mag_temp_data *data,
struct bmm350_oor_params *oor,
struct bmm350_dev *dev);
/*!
* @brief Function to perform reset sequence in forced mode.
*
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
* @param[in,out] oor : Structure that stores the state of the out of range detector
* @param[in,out] dev : Structure instance of bmm350_dev.
*
* @return Result of API execution status
* @retval = 0 -> Success
* @retval < 0 -> Error
*/
int8_t bmm350_oor_perform_reset_sequence(const struct bmm350_oor_reset_delay *rdelay,
struct bmm350_oor_params *oor,
struct bmm350_dev *dev);
/*!
* @brief Function to compute the delay settings for the magnetic reset sequence.
*
* @param[in] odr_config : Configured Output Data Rate
* @param[in] rdelay : Structure that stores the delay settings for the various magnetic reset sequences
* @param[in,out] dev : Structure instance of bmm350_dev.
*/
void bmm350_oor_compute_delay_setting(uint8_t odr_config, struct bmm350_oor_reset_delay *rdelay);
/*!
* @brief Function to check for the Out of Range condition.
*
* @param[out] data : Sensor data
* @param[in,out] oor : Structure that stores the state of the out of range detector
*/
void bmm350_oor_validate_out_of_range(const struct bmm350_mag_temp_data *data, struct bmm350_oor_params *oor);
#ifdef __cplusplus
}
#endif /* End of CPP guard */
#endif /* _BMM350_OOR_H */
Core/Src/freertos_port.c
+322
View File
@@ -0,0 +1,322 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file freertos_port.c
* @author MCD Application Team
* @brief Custom porting of FreeRTOS functionalities
******************************************************************************
* @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 "app_common.h"
#include "FreeRTOS.h"
#include "task.h"
#include "stm32_lpm.h"
#include <limits.h>
/* Private typedef -----------------------------------------------------------*/
typedef struct
{
uint32_t LpTimeLeftOnEntry;
uint8_t LpTimerFreeRTOS_Id;
} LpTimerContext_t;
/* Private defines -----------------------------------------------------------*/
#ifndef configSYSTICK_CLOCK_HZ
#define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ
/* Ensure the SysTick is clocked at the same frequency as the core. */
#define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL )
#else
/* The way the SysTick is clocked is not modified in case it is not the same
as the core. */
#define portNVIC_SYSTICK_CLK_BIT ( 0 )
#endif
#define CPU_CLOCK_KHZ ( configCPU_CLOCK_HZ / 1000 )
/* Constants required to manipulate the core. Registers first... */
#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000e010 ) )
#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile uint32_t * ) 0xe000e014 ) )
#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile uint32_t * ) 0xe000e018 ) )
#define portNVIC_SYSTICK_INT_BIT ( 1UL << 1UL )
#define portNVIC_SYSTICK_ENABLE_BIT ( 1UL << 0UL )
#define portNVIC_SYSTICK_COUNT_FLAG_BIT ( 1UL << 16UL )
/* Private macros ------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/*
* The number of SysTick increments that make up one tick period.
*/
#if ( CFG_LPM_SUPPORTED != 0)
static uint32_t ulTimerCountsForOneTick;
static LpTimerContext_t LpTimerContext;
#endif
/* Global variables ----------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
#if ( CFG_LPM_SUPPORTED != 0)
static void LpTimerInit( void );
static void LpTimerCb( void );
static void LpTimerStart( uint32_t time_to_sleep );
static void LpEnter( void );
static uint32_t LpGetElapsedTime( void );
void vPortSetupTimerInterrupt( void );
#endif
/* Functions Definition ------------------------------------------------------*/
/**
* @brief Implement the tickless feature
*
*
* @param: xExpectedIdleTime is given in number of FreeRTOS Ticks
* @retval: None
*/
void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime )
{
/* If low power is not used, do not stop the SysTick and continue execution */
#if ( CFG_LPM_SUPPORTED != 0)
/**
* Although this is not documented as such, when xExpectedIdleTime = 0xFFFFFFFF = (~0),
* it likely means the system may enter low power for ever ( from a FreeRTOS point of view ).
* Otherwise, for a FreeRTOS tick set to 1ms, that would mean it is requested to wakeup in 8 years from now.
* When the system may enter low power mode for ever, FreeRTOS is not really interested to maintain a
* systick count and when the system exits from low power mode, there is no need to update the count with
* the time spent in low power mode
*/
uint32_t ulCompleteTickPeriods;
/* Stop the SysTick to avoid the interrupt to occur while in the critical section.
* Otherwise, this will prevent the device to enter low power mode
* At this time, an update of the systick will not be considered
*
*/
portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT;
/* Enter a critical section but don't use the taskENTER_CRITICAL()
method as that will mask interrupts that should exit sleep mode. */
__disable_irq();
__DSB();
__ISB();
/* If a context switch is pending or a task is waiting for the scheduler
to be unsuspended then abandon the low power entry. */
if( eTaskConfirmSleepModeStatus() == eAbortSleep )
{
/* Restart SysTick. */
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
/* Re-enable interrupts - see comments above __disable_interrupt()
call above. */
__enable_irq();
}
else
{
if (xExpectedIdleTime != (~0))
{
/* Remove one tick to wake up before the event occurs */
xExpectedIdleTime--;
/* Start the low power timer */
LpTimerStart( xExpectedIdleTime );
}
/* Enter low power mode */
LpEnter( );
if (xExpectedIdleTime != (~0))
{
/**
* Get the number of FreeRTOS ticks that has been suppressed
* In the current implementation, this shall be kept in critical section
* so that the timer server return the correct elapsed time
*/
ulCompleteTickPeriods = LpGetElapsedTime( );
vTaskStepTick( ulCompleteTickPeriods );
}
/* Restart SysTick */
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT;
portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL;
/* Exit with interrUpts enabled. */
__enable_irq();
}
#endif
}
/*
* Setup the systick timer to generate the tick interrupts at the required
* frequency and initialize a low power timer
* The current implementation is kept as close as possible to the default tickless
* mode provided.
* The systick is still used when there is no need to go in low power mode.
* When the system needs to enter low power mode, the tick is suppressed and a low power timer
* is used over that time
* Note that in sleep mode, the system clock is still running and the default tickless implementation
* using systick could have been kept.
* However, as at that time, it is not yet known whereas the low power mode that will be used is stop mode or
* sleep mode, it is easier and simpler to go with a low power timer as soon as the tick need to be
* suppressed.
*/
#if ( CFG_LPM_SUPPORTED != 0)
void vPortSetupTimerInterrupt( void )
{
LpTimerInit( );
/* Calculate the constants required to configure the tick interrupt. */
ulTimerCountsForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ );
/* Stop and clear the SysTick. */
portNVIC_SYSTICK_CTRL_REG = 0UL;
portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;
/* Configure SysTick to interrupt at the requested rate. */
portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;
portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT );
}
#endif
/**
* @brief The current implementation uses the hw_timerserver to provide a low power timer
* This may be replaced by another low power timer.
*
* @param None
* @retval None
*/
#if ( CFG_LPM_SUPPORTED != 0)
static void LpTimerInit( void )
{
( void ) HW_TS_Create(CFG_TIM_PROC_ID_ISR, &(LpTimerContext.LpTimerFreeRTOS_Id), hw_ts_SingleShot, LpTimerCb);
return;
}
#endif
/**
* @brief Low power timer callback
*
* @param None
* @retval None
*/
#if ( CFG_LPM_SUPPORTED != 0)
static void LpTimerCb( void )
{
/**
* Nothing to be done
*/
return;
}
#endif
/**
* @brief Request to start a low power timer ( running is stop mode )
*
* @param time_to_sleep : Number of FreeRTOS ticks
* @retval None
*/
#if ( CFG_LPM_SUPPORTED != 0)
static void LpTimerStart( uint32_t time_to_sleep )
{
uint64_t time;
/* Converts the number of FreeRTOS ticks into hw timer tick */
if (time_to_sleep > (ULLONG_MAX / 1e12)) /* Prevent overflow in else statement */
{
time = 0xFFFF0000; /* Maximum value equal to 24 days */
}
else
{
/* The result always fits in uint32_t and is always less than 0xFFFF0000 */
time = time_to_sleep * 1000000000000ULL;
time = (uint64_t)( time / ( CFG_TS_TICK_VAL_PS * configTICK_RATE_HZ ));
}
HW_TS_Start(LpTimerContext.LpTimerFreeRTOS_Id, (uint32_t)time);
/**
* There might be other timers already running in the timer server that may elapse
* before this one.
* Store how long before the next event so that on wakeup, it will be possible to calculate
* how long the tick has been suppressed
*/
LpTimerContext.LpTimeLeftOnEntry = HW_TS_RTC_ReadLeftTicksToCount( );
return;
}
#endif
/**
* @brief Enter low power mode
*
* @param None
* @retval None
*/
#if ( CFG_LPM_SUPPORTED != 0)
static void LpEnter( void )
{
#if ( CFG_LPM_SUPPORTED == 1)
UTIL_LPM_EnterLowPower();
#endif
return;
}
#endif
/**
* @brief Read how long the tick has been suppressed
*
* @param None
* @retval The number of tick rate (FreeRTOS tick)
*/
#if ( CFG_LPM_SUPPORTED != 0)
static uint32_t LpGetElapsedTime( void )
{
uint64_t val_ticks, time_ps;
uint32_t LpTimeLeftOnExit;
LpTimeLeftOnExit = HW_TS_RTC_ReadLeftTicksToCount();
/* This cannot overflow. Max result is ~ 1.6e13 */
time_ps = (uint64_t)((CFG_TS_TICK_VAL_PS) * (uint64_t)(LpTimerContext.LpTimeLeftOnEntry - LpTimeLeftOnExit));
/* time_ps can be less than 1 RTOS tick in following situations
* a) MCU didn't go to STOP2 due to wake-up unrelated to Timer Server or woke up from STOP2 very shortly after.
* Advancing RTOS clock by 1 FreeRTOS tick doesn't hurt in this case.
* b) vPortSuppressTicksAndSleep(xExpectedIdleTime) was called with xExpectedIdleTime = 2 which is minimum value defined by configEXPECTED_IDLE_TIME_BEFORE_SLEEP.
* The xExpectedIdleTime is decremented by one RTOS tick to wake-up in advance.
* Ex: RTOS tick is 1ms, the timer Server wakes the MCU in ~977 us. RTOS clock should be advanced by 1 ms.
* */
if(time_ps <= (1e12 / configTICK_RATE_HZ)) /* time_ps < RTOS tick */
{
val_ticks = 1;
}
else
{
/* Convert pS time into OS ticks */
val_ticks = time_ps * configTICK_RATE_HZ; /* This cannot overflow. Max result is ~ 1.6e16 */
val_ticks = (uint64_t)(val_ticks / (1e12)); /* The result always fits in uint32_t */
}
/**
* The system may have been out from another reason than the timer
* Stop the timer after the elapsed time is calculated other wise, HW_TS_RTC_ReadLeftTicksToCount()
* may return 0xFFFF ( TIMER LIST EMPTY )
* It does not hurt stopping a timer that exists but is not running.
*/
HW_TS_Stop(LpTimerContext.LpTimerFreeRTOS_Id);
return (uint32_t)val_ticks;
}
#endif
Core/Src/hw_timerserver.c
+888
View File
@@ -0,0 +1,888 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file hw_timerserver.c
* @author MCD Application Team
* @brief Hardware timerserver source file for STM32WPAN Middleware.
******************************************************************************
* @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 "app_common.h"
#include "hw_conf.h"
/* Private typedef -----------------------------------------------------------*/
typedef enum
{
TimerID_Free,
TimerID_Created,
TimerID_Running
}TimerIDStatus_t;
typedef enum
{
SSR_Read_Requested,
SSR_Read_Not_Requested
}RequestReadSSR_t;
typedef enum
{
WakeupTimerValue_Overpassed,
WakeupTimerValue_LargeEnough
}WakeupTimerLimitation_Status_t;
typedef struct
{
HW_TS_pTimerCb_t pTimerCallBack;
uint32_t CounterInit;
uint32_t CountLeft;
TimerIDStatus_t TimerIDStatus;
HW_TS_Mode_t TimerMode;
uint32_t TimerProcessID;
uint8_t PreviousID;
uint8_t NextID;
}TimerContext_t;
/* Private defines -----------------------------------------------------------*/
#define SSR_FORBIDDEN_VALUE 0xFFFFFFFF
#define TIMER_LIST_EMPTY 0xFFFF
/* Private macros ------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/**
* START of Section TIMERSERVER_CONTEXT
*/
static volatile TimerContext_t aTimerContext[CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER];
static volatile uint8_t CurrentRunningTimerID;
static volatile uint8_t PreviousRunningTimerID;
static volatile uint32_t SSRValueOnLastSetup;
static volatile WakeupTimerLimitation_Status_t WakeupTimerLimitation;
/**
* END of Section TIMERSERVER_CONTEXT
*/
static uint8_t WakeupTimerDivider;
static uint8_t AsynchPrescalerUserConfig;
static uint16_t SynchPrescalerUserConfig;
static volatile uint16_t MaxWakeupTimerSetup;
/* Global variables ----------------------------------------------------------*/
extern RTC_HandleTypeDef hrtc;
/* Private function prototypes -----------------------------------------------*/
static void RestartWakeupCounter(uint16_t Value);
static uint16_t ReturnTimeElapsed(void);
static void RescheduleTimerList(void);
static void UnlinkTimer(uint8_t TimerID, RequestReadSSR_t RequestReadSSR);
static void LinkTimerBefore(uint8_t TimerID, uint8_t RefTimerID);
static void LinkTimerAfter(uint8_t TimerID, uint8_t RefTimerID);
static uint16_t linkTimer(uint8_t TimerID);
static uint32_t ReadRtcSsrValue(void);
__weak void HW_TS_RTC_CountUpdated_AppNot(void);
/* Functions Definition ------------------------------------------------------*/
/**
* @brief Read the RTC_SSR value
* As described in the reference manual, the RTC_SSR shall be read twice to ensure
* reliability of the value
* @param None
* @retval SSR value read
*/
static uint32_t ReadRtcSsrValue(void)
{
uint32_t first_read;
uint32_t second_read;
first_read = (uint32_t)(READ_BIT(RTC->SSR, RTC_SSR_SS));
second_read = (uint32_t)(READ_BIT(RTC->SSR, RTC_SSR_SS));
while(first_read != second_read)
{
first_read = second_read;
second_read = (uint32_t)(READ_BIT(RTC->SSR, RTC_SSR_SS));
}
return second_read;
}
/**
* @brief Insert a Timer in the list after the Timer ID specified
* @param TimerID: The ID of the Timer
* @param RefTimerID: The ID of the Timer to be linked after
* @retval None
*/
static void LinkTimerAfter(uint8_t TimerID, uint8_t RefTimerID)
{
uint8_t next_id;
next_id = aTimerContext[RefTimerID].NextID;
if(next_id != CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER)
{
aTimerContext[next_id].PreviousID = TimerID;
}
aTimerContext[TimerID].NextID = next_id;
aTimerContext[TimerID].PreviousID = RefTimerID ;
aTimerContext[RefTimerID].NextID = TimerID;
return;
}
/**
* @brief Insert a Timer in the list before the ID specified
* @param TimerID: The ID of the Timer
* @param RefTimerID: The ID of the Timer to be linked before
* @retval None
*/
static void LinkTimerBefore(uint8_t TimerID, uint8_t RefTimerID)
{
uint8_t previous_id;
if(RefTimerID != CurrentRunningTimerID)
{
previous_id = aTimerContext[RefTimerID].PreviousID;
aTimerContext[previous_id].NextID = TimerID;
aTimerContext[TimerID].NextID = RefTimerID;
aTimerContext[TimerID].PreviousID = previous_id ;
aTimerContext[RefTimerID].PreviousID = TimerID;
}
else
{
aTimerContext[TimerID].NextID = RefTimerID;
aTimerContext[RefTimerID].PreviousID = TimerID;
}
return;
}
/**
* @brief Insert a Timer in the list
* @param TimerID: The ID of the Timer
* @retval None
*/
static uint16_t linkTimer(uint8_t TimerID)
{
uint32_t time_left;
uint16_t time_elapsed;
uint8_t timer_id_lookup;
uint8_t next_id;
if(CurrentRunningTimerID == CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER)
{
/**
* No timer in the list
*/
PreviousRunningTimerID = CurrentRunningTimerID;
CurrentRunningTimerID = TimerID;
aTimerContext[TimerID].NextID = CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER;
SSRValueOnLastSetup = SSR_FORBIDDEN_VALUE;
time_elapsed = 0;
}
else
{
time_elapsed = ReturnTimeElapsed();
/**
* update count of the timer to be linked
*/
aTimerContext[TimerID].CountLeft += time_elapsed;
time_left = aTimerContext[TimerID].CountLeft;
/**
* Search for index where the new timer shall be linked
*/
if(aTimerContext[CurrentRunningTimerID].CountLeft <= time_left)
{
/**
* Search for the ID after the first one
*/
timer_id_lookup = CurrentRunningTimerID;
next_id = aTimerContext[timer_id_lookup].NextID;
while((next_id != CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER) && (aTimerContext[next_id].CountLeft <= time_left))
{
timer_id_lookup = aTimerContext[timer_id_lookup].NextID;
next_id = aTimerContext[timer_id_lookup].NextID;
}
/**
* Link after the ID
*/
LinkTimerAfter(TimerID, timer_id_lookup);
}
else
{
/**
* Link before the first ID
*/
LinkTimerBefore(TimerID, CurrentRunningTimerID);
PreviousRunningTimerID = CurrentRunningTimerID;
CurrentRunningTimerID = TimerID;
}
}
return time_elapsed;
}
/**
* @brief Remove a Timer from the list
* @param TimerID: The ID of the Timer
* @param RequestReadSSR: Request to read the SSR register or not
* @retval None
*/
static void UnlinkTimer(uint8_t TimerID, RequestReadSSR_t RequestReadSSR)
{
uint8_t previous_id;
uint8_t next_id;
if(TimerID == CurrentRunningTimerID)
{
PreviousRunningTimerID = CurrentRunningTimerID;
CurrentRunningTimerID = aTimerContext[TimerID].NextID;
}
else
{
previous_id = aTimerContext[TimerID].PreviousID;
next_id = aTimerContext[TimerID].NextID;
aTimerContext[previous_id].NextID = aTimerContext[TimerID].NextID;
if(next_id != CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER)
{
aTimerContext[next_id].PreviousID = aTimerContext[TimerID].PreviousID;
}
}
/**
* Timer is out of the list
*/
aTimerContext[TimerID].TimerIDStatus = TimerID_Created;
if((CurrentRunningTimerID == CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER) && (RequestReadSSR == SSR_Read_Requested))
{
SSRValueOnLastSetup = SSR_FORBIDDEN_VALUE;
}
return;
}
/**
* @brief Return the number of ticks counted by the wakeuptimer since it has been started
* @note The API is reading the SSR register to get how many ticks have been counted
* since the time the timer has been started
* @param None
* @retval Time expired in Ticks
*/
static uint16_t ReturnTimeElapsed(void)
{
uint32_t return_value;
uint32_t wrap_counter;
if(SSRValueOnLastSetup != SSR_FORBIDDEN_VALUE)
{
return_value = ReadRtcSsrValue(); /**< Read SSR register first */
if (SSRValueOnLastSetup >= return_value)
{
return_value = SSRValueOnLastSetup - return_value;
}
else
{
wrap_counter = SynchPrescalerUserConfig - return_value;
return_value = SSRValueOnLastSetup + wrap_counter;
}
/**
* At this stage, ReturnValue holds the number of ticks counted by SSR
* Need to translate in number of ticks counted by the Wakeuptimer
*/
return_value = return_value*AsynchPrescalerUserConfig;
return_value = return_value >> WakeupTimerDivider;
}
else
{
return_value = 0;
}
return (uint16_t)return_value;
}
/**
* @brief Set the wakeup counter
* @note The API is writing the counter value so that the value is decreased by one to cope with the fact
* the interrupt is generated with 1 extra clock cycle (See RefManuel)
* It assumes all condition are met to be allowed to write the wakeup counter
* @param Value: Value to be written in the counter
* @retval None
*/
static void RestartWakeupCounter(uint16_t Value)
{
/**
* The wakeuptimer has been disabled in the calling function to reduce the time to poll the WUTWF
* FLAG when the new value will have to be written
* __HAL_RTC_WAKEUPTIMER_DISABLE(&hrtc);
*/
if(Value == 0)
{
SSRValueOnLastSetup = ReadRtcSsrValue();
/**
* Simulate that the Timer expired
*/
HAL_NVIC_SetPendingIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID);
}
else
{
if((Value > 1) ||(WakeupTimerDivider != 1))
{
Value -= 1;
}
while(__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTWF) == RESET);
/**
* make sure to clear the flags after checking the WUTWF.
* It takes 2 RTCCLK between the time the WUTE bit is disabled and the
* time the timer is disabled. The WUTWF bit somehow guarantee the system is stable
* Otherwise, when the timer is periodic with 1 Tick, it may generate an extra interrupt in between
* due to the autoreload feature
*/
__HAL_RTC_WAKEUPTIMER_CLEAR_FLAG(&hrtc, RTC_FLAG_WUTF); /**< Clear flag in RTC module */
__HAL_RTC_WAKEUPTIMER_EXTI_CLEAR_FLAG(); /**< Clear flag in EXTI module */
HAL_NVIC_ClearPendingIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Clear pending bit in NVIC */
MODIFY_REG(RTC->WUTR, RTC_WUTR_WUT, Value);
/**
* Update the value here after the WUTWF polling that may take some time
*/
SSRValueOnLastSetup = ReadRtcSsrValue();
__HAL_RTC_WAKEUPTIMER_ENABLE(&hrtc); /**< Enable the Wakeup Timer */
HW_TS_RTC_CountUpdated_AppNot();
}
return ;
}
/**
* @brief Reschedule the list of timer
* @note 1) Update the count left for each timer in the list
* 2) Setup the wakeuptimer
* @param None
* @retval None
*/
static void RescheduleTimerList(void)
{
uint8_t localTimerID;
uint32_t timecountleft;
uint16_t wakeup_timer_value;
uint16_t time_elapsed;
/**
* The wakeuptimer is disabled now to reduce the time to poll the WUTWF
* FLAG when the new value will have to be written
*/
if((READ_BIT(RTC->CR, RTC_CR_WUTE) == (RTC_CR_WUTE)) == SET)
{
/**
* Wait for the flag to be back to 0 when the wakeup timer is enabled
*/
while(__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTWF) == SET);
}
__HAL_RTC_WAKEUPTIMER_DISABLE(&hrtc); /**< Disable the Wakeup Timer */
localTimerID = CurrentRunningTimerID;
/**
* Calculate what will be the value to write in the wakeuptimer
*/
timecountleft = aTimerContext[localTimerID].CountLeft;
/**
* Read how much has been counted
*/
time_elapsed = ReturnTimeElapsed();
if(timecountleft < time_elapsed )
{
/**
* There is no tick left to count
*/
wakeup_timer_value = 0;
WakeupTimerLimitation = WakeupTimerValue_LargeEnough;
}
else
{
if(timecountleft > (time_elapsed + MaxWakeupTimerSetup))
{
/**
* The number of tick left is greater than the Wakeuptimer maximum value
*/
wakeup_timer_value = MaxWakeupTimerSetup;
WakeupTimerLimitation = WakeupTimerValue_Overpassed;
}
else
{
wakeup_timer_value = timecountleft - time_elapsed;
WakeupTimerLimitation = WakeupTimerValue_LargeEnough;
}
}
/**
* update ticks left to be counted for each timer
*/
while(localTimerID != CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER)
{
if (aTimerContext[localTimerID].CountLeft < time_elapsed)
{
aTimerContext[localTimerID].CountLeft = 0;
}
else
{
aTimerContext[localTimerID].CountLeft -= time_elapsed;
}
localTimerID = aTimerContext[localTimerID].NextID;
}
/**
* Write next count
*/
RestartWakeupCounter(wakeup_timer_value);
return ;
}
/* Public functions ----------------------------------------------------------*/
/**
* For all public interface except that may need write access to the RTC, the RTC
* shall be unlock at the beginning and locked at the output
* In order to ease maintainability, the unlock is done at the top and the lock at then end
* in case some new implementation is coming in the future
*/
void HW_TS_RTC_Wakeup_Handler(void)
{
HW_TS_pTimerCb_t ptimer_callback;
uint32_t timer_process_id;
uint8_t local_current_running_timer_id;
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
uint32_t primask_bit;
#endif
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
#endif
/* Disable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_DISABLE( &hrtc );
/**
* Disable the Wakeup Timer
* This may speed up a bit the processing to wait the timer to be disabled
* The timer is still counting 2 RTCCLK
*/
__HAL_RTC_WAKEUPTIMER_DISABLE(&hrtc);
local_current_running_timer_id = CurrentRunningTimerID;
if(aTimerContext[local_current_running_timer_id].TimerIDStatus == TimerID_Running)
{
ptimer_callback = aTimerContext[local_current_running_timer_id].pTimerCallBack;
timer_process_id = aTimerContext[local_current_running_timer_id].TimerProcessID;
/**
* It should be good to check whether the TimeElapsed is greater or not than the tick left to be counted
* However, due to the inaccuracy of the reading of the time elapsed, it may return there is 1 tick
* to be left whereas the count is over
* A more secure implementation has been done with a flag to state whereas the full count has been written
* in the wakeuptimer or not
*/
if(WakeupTimerLimitation != WakeupTimerValue_Overpassed)
{
if(aTimerContext[local_current_running_timer_id].TimerMode == hw_ts_Repeated)
{
UnlinkTimer(local_current_running_timer_id, SSR_Read_Not_Requested);
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
HW_TS_Start(local_current_running_timer_id, aTimerContext[local_current_running_timer_id].CounterInit);
/* Disable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_DISABLE( &hrtc );
}
else
{
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
HW_TS_Stop(local_current_running_timer_id);
/* Disable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_DISABLE( &hrtc );
}
HW_TS_RTC_Int_AppNot(timer_process_id, local_current_running_timer_id, ptimer_callback);
}
else
{
RescheduleTimerList();
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
}
}
else
{
/**
* We should never end up in this case
* However, if due to any bug in the timer server this is the case, the mistake may not impact the user.
* We could just clean the interrupt flag and get out from this unexpected interrupt
*/
while(__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTWF) == RESET);
/**
* make sure to clear the flags after checking the WUTWF.
* It takes 2 RTCCLK between the time the WUTE bit is disabled and the
* time the timer is disabled. The WUTWF bit somehow guarantee the system is stable
* Otherwise, when the timer is periodic with 1 Tick, it may generate an extra interrupt in between
* due to the autoreload feature
*/
__HAL_RTC_WAKEUPTIMER_CLEAR_FLAG(&hrtc, RTC_FLAG_WUTF); /**< Clear flag in RTC module */
__HAL_RTC_WAKEUPTIMER_EXTI_CLEAR_FLAG(); /**< Clear flag in EXTI module */
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
}
/* Enable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_ENABLE( &hrtc );
return;
}
void HW_TS_Init(HW_TS_InitMode_t TimerInitMode, RTC_HandleTypeDef *phrtc)
{
uint8_t loop;
uint32_t localmaxwakeuptimersetup;
/* Disable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_DISABLE( &hrtc );
SET_BIT(RTC->CR, RTC_CR_BYPSHAD);
/**
* Readout the user config
*/
WakeupTimerDivider = (4 - ((uint32_t)(READ_BIT(RTC->CR, RTC_CR_WUCKSEL))));
AsynchPrescalerUserConfig = (uint8_t)(READ_BIT(RTC->PRER, RTC_PRER_PREDIV_A) >> (uint32_t)POSITION_VAL(RTC_PRER_PREDIV_A)) + 1;
SynchPrescalerUserConfig = (uint16_t)(READ_BIT(RTC->PRER, RTC_PRER_PREDIV_S)) + 1;
/**
* Margin is taken to avoid wrong calculation when the wrap around is there and some
* application interrupts may have delayed the reading
*/
localmaxwakeuptimersetup = ((((SynchPrescalerUserConfig - 1)*AsynchPrescalerUserConfig) - CFG_HW_TS_RTC_HANDLER_MAX_DELAY) >> WakeupTimerDivider);
if(localmaxwakeuptimersetup >= 0xFFFF)
{
MaxWakeupTimerSetup = 0xFFFF;
}
else
{
MaxWakeupTimerSetup = (uint16_t)localmaxwakeuptimersetup;
}
/**
* Configure EXTI module
*/
LL_EXTI_EnableRisingTrig_0_31(RTC_EXTI_LINE_WAKEUPTIMER_EVENT);
LL_EXTI_EnableIT_0_31(RTC_EXTI_LINE_WAKEUPTIMER_EVENT);
if(TimerInitMode == hw_ts_InitMode_Full)
{
WakeupTimerLimitation = WakeupTimerValue_LargeEnough;
SSRValueOnLastSetup = SSR_FORBIDDEN_VALUE;
/**
* Initialize the timer server
*/
for(loop = 0; loop < CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER; loop++)
{
aTimerContext[loop].TimerIDStatus = TimerID_Free;
}
CurrentRunningTimerID = CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER; /**< Set ID to non valid value */
__HAL_RTC_WAKEUPTIMER_DISABLE(&hrtc); /**< Disable the Wakeup Timer */
__HAL_RTC_WAKEUPTIMER_CLEAR_FLAG(&hrtc, RTC_FLAG_WUTF); /**< Clear flag in RTC module */
__HAL_RTC_WAKEUPTIMER_EXTI_CLEAR_FLAG(); /**< Clear flag in EXTI module */
HAL_NVIC_ClearPendingIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Clear pending bit in NVIC */
__HAL_RTC_WAKEUPTIMER_ENABLE_IT(&hrtc, RTC_IT_WUT); /**< Enable interrupt in RTC module */
}
else
{
if(__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTF) != RESET)
{
/**
* Simulate that the Timer expired
*/
HAL_NVIC_SetPendingIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID);
}
}
/* Enable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_ENABLE( &hrtc );
HAL_NVIC_SetPriority(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID, CFG_HW_TS_NVIC_RTC_WAKEUP_IT_PREEMPTPRIO, CFG_HW_TS_NVIC_RTC_WAKEUP_IT_SUBPRIO); /**< Set NVIC priority */
HAL_NVIC_EnableIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Enable NVIC */
return;
}
HW_TS_ReturnStatus_t HW_TS_Create(uint32_t TimerProcessID, uint8_t *pTimerId, HW_TS_Mode_t TimerMode, HW_TS_pTimerCb_t pftimeout_handler)
{
HW_TS_ReturnStatus_t localreturnstatus;
uint8_t loop = 0;
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
uint32_t primask_bit;
#endif
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
#endif
while((loop < CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER) && (aTimerContext[loop].TimerIDStatus != TimerID_Free))
{
loop++;
}
if(loop != CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER)
{
aTimerContext[loop].TimerIDStatus = TimerID_Created;
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
aTimerContext[loop].TimerProcessID = TimerProcessID;
aTimerContext[loop].TimerMode = TimerMode;
aTimerContext[loop].pTimerCallBack = pftimeout_handler;
*pTimerId = loop;
localreturnstatus = hw_ts_Successful;
}
else
{
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
localreturnstatus = hw_ts_Failed;
}
return(localreturnstatus);
}
void HW_TS_Delete(uint8_t timer_id)
{
HW_TS_Stop(timer_id);
aTimerContext[timer_id].TimerIDStatus = TimerID_Free; /**< release ID */
return;
}
void HW_TS_Stop(uint8_t timer_id)
{
uint8_t localcurrentrunningtimerid;
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
uint32_t primask_bit;
#endif
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
#endif
HAL_NVIC_DisableIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Disable NVIC */
/* Disable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_DISABLE( &hrtc );
if(aTimerContext[timer_id].TimerIDStatus == TimerID_Running)
{
UnlinkTimer(timer_id, SSR_Read_Requested);
localcurrentrunningtimerid = CurrentRunningTimerID;
if(localcurrentrunningtimerid == CFG_HW_TS_MAX_NBR_CONCURRENT_TIMER)
{
/**
* List is empty
*/
/**
* Disable the timer
*/
if((READ_BIT(RTC->CR, RTC_CR_WUTE) == (RTC_CR_WUTE)) == SET)
{
/**
* Wait for the flag to be back to 0 when the wakeup timer is enabled
*/
while(__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTWF) == SET);
}
__HAL_RTC_WAKEUPTIMER_DISABLE(&hrtc); /**< Disable the Wakeup Timer */
while(__HAL_RTC_WAKEUPTIMER_GET_FLAG(&hrtc, RTC_FLAG_WUTWF) == RESET);
/**
* make sure to clear the flags after checking the WUTWF.
* It takes 2 RTCCLK between the time the WUTE bit is disabled and the
* time the timer is disabled. The WUTWF bit somehow guarantee the system is stable
* Otherwise, when the timer is periodic with 1 Tick, it may generate an extra interrupt in between
* due to the autoreload feature
*/
__HAL_RTC_WAKEUPTIMER_CLEAR_FLAG(&hrtc, RTC_FLAG_WUTF); /**< Clear flag in RTC module */
__HAL_RTC_WAKEUPTIMER_EXTI_CLEAR_FLAG(); /**< Clear flag in EXTI module */
HAL_NVIC_ClearPendingIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Clear pending bit in NVIC */
}
else if(PreviousRunningTimerID != localcurrentrunningtimerid)
{
RescheduleTimerList();
}
}
/* Enable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_ENABLE( &hrtc );
HAL_NVIC_EnableIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Enable NVIC */
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
return;
}
void HW_TS_Start(uint8_t timer_id, uint32_t timeout_ticks)
{
uint16_t time_elapsed;
uint8_t localcurrentrunningtimerid;
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
uint32_t primask_bit;
#endif
if(aTimerContext[timer_id].TimerIDStatus == TimerID_Running)
{
HW_TS_Stop( timer_id );
}
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
#endif
HAL_NVIC_DisableIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Disable NVIC */
/* Disable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_DISABLE( &hrtc );
aTimerContext[timer_id].TimerIDStatus = TimerID_Running;
aTimerContext[timer_id].CountLeft = timeout_ticks;
aTimerContext[timer_id].CounterInit = timeout_ticks;
time_elapsed = linkTimer(timer_id);
localcurrentrunningtimerid = CurrentRunningTimerID;
if(PreviousRunningTimerID != localcurrentrunningtimerid)
{
RescheduleTimerList();
}
else
{
aTimerContext[timer_id].CountLeft -= time_elapsed;
}
/* Enable the write protection for RTC registers */
__HAL_RTC_WRITEPROTECTION_ENABLE( &hrtc );
HAL_NVIC_EnableIRQ(CFG_HW_TS_RTC_WAKEUP_HANDLER_ID); /**< Enable NVIC */
#if (CFG_HW_TS_USE_PRIMASK_AS_CRITICAL_SECTION == 1)
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
#endif
return;
}
uint16_t HW_TS_RTC_ReadLeftTicksToCount(void)
{
uint32_t primask_bit;
uint16_t return_value, auro_reload_value, elapsed_time_value;
primask_bit = __get_PRIMASK(); /**< backup PRIMASK bit */
__disable_irq(); /**< Disable all interrupts by setting PRIMASK bit on Cortex*/
if((READ_BIT(RTC->CR, RTC_CR_WUTE) == (RTC_CR_WUTE)) == SET)
{
auro_reload_value = (uint32_t)(READ_BIT(RTC->WUTR, RTC_WUTR_WUT));
elapsed_time_value = ReturnTimeElapsed();
if(auro_reload_value > elapsed_time_value)
{
return_value = auro_reload_value - elapsed_time_value;
}
else
{
return_value = 0;
}
}
else
{
return_value = TIMER_LIST_EMPTY;
}
__set_PRIMASK(primask_bit); /**< Restore PRIMASK bit*/
return (return_value);
}
__weak void HW_TS_RTC_Int_AppNot(uint32_t TimerProcessID, uint8_t TimerID, HW_TS_pTimerCb_t pTimerCallBack)
{
pTimerCallBack();
return;
}
Core/Src/madgwick.c
+117
View File
@@ -0,0 +1,117 @@
#include "madgwick.h"
#include <math.h>
// Initialize the filter
void MadgwickFilter_Init(MadgwickFilter* filter, float sampleFreq, float beta) {
filter->q[0] = 1.0f; // qw (scalar component)
filter->q[1] = 0.0f; // qx
filter->q[2] = 0.0f; // qy
filter->q[3] = 0.0f; // qz
filter->beta = beta; // Filter gain (e.g., 0.1)
filter->sampleFreq = sampleFreq; // Sampling frequency in Hz
}
// Update the quaternion using accelerometer and gyroscope data
void MadgwickFilter_Update(MadgwickFilter* filter, float gx, float gy, float gz,
float ax, float ay, float az) {
float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
float recipNorm;
float s[4];
float qDot[4];
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Convert gyroscope data from degrees/s to radians/s
gx *= 0.0174533f; // deg/s to rad/s
gy *= 0.0174533f;
gz *= 0.0174533f;
// Rate of change of quaternion from gyroscope
qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * gx);
// Compute accelerometer objective function and Jacobian
if ((ax != 0.0f) || (ay != 0.0f) || (az != 0.0f)) {
// Normalize accelerometer measurement
recipNorm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated calculations
_2q0 = 2.0f * q[0];
_2q1 = 2.0f * q[1];
_2q2 = 2.0f * q[2];
_2q3 = 2.0f * q[3];
_4q0 = 4.0f * q[0];
_4q1 = 4.0f * q[1];
_4q2 = 4.0f * q[2];
_8q1 = 8.0f * q[1];
_8q2 = 8.0f * q[2];
q0q0 = q[0] * q[0];
q1q1 = q[1] * q[1];
q2q2 = q[2] * q[2];
q3q3 = q[3] * q[3];
// Gradient descent algorithm corrective step
s[0] = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s[1] = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s[2] = 4.0f * q0q0 * q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s[3] = 4.0f * q1q1 * q[3] - _2q1 * ax + 4.0f * q2q2 * q[3] - _2q2 * ay;
// Normalize step magnitude
recipNorm = 1.0f / sqrtf(s[0] * s[0] + s[1] * s[1] + s[2] * s[2] + s[3] * s[3]);
s[0] *= recipNorm;
s[1] *= recipNorm;
s[2] *= recipNorm;
s[3] *= recipNorm;
// Apply feedback step
qDot[0] -= filter->beta * s[0];
qDot[1] -= filter->beta * s[1];
qDot[2] -= filter->beta * s[2];
qDot[3] -= filter->beta * s[3];
}
// Integrate rate of change of quaternion
float dt = 1.0f / filter->sampleFreq;
q[0] += qDot[0] * dt;
q[1] += qDot[1] * dt;
q[2] += qDot[2] * dt;
q[3] += qDot[3] * dt;
// Normalize quaternion
recipNorm = 1.0f / sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
// Update filter state
filter->q[0] = q[0];
filter->q[1] = q[1];
filter->q[2] = q[2];
filter->q[3] = q[3];
}
// Convert quaternion to Euler angles (roll, pitch, yaw) in degrees
void MadgwickFilter_GetEulerAngles(MadgwickFilter* filter, float* roll, float* pitch, float* yaw) {
float q0 = filter->q[0];
float q1 = filter->q[1];
float q2 = filter->q[2];
float q3 = filter->q[3];
// Roll (x-axis rotation)
*roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.2958f;
// Pitch (y-axis rotation)
float sinp = 2.0f * (q0 * q2 - q3 * q1);
if (fabsf(sinp) >= 1)
*pitch = copysignf(M_PI / 2, sinp) * 57.2958f; // Use 90 degrees if out of range
else
*pitch = asinf(sinp) * 57.2958f;
// Yaw (z-axis rotation) - Note: Without magnetometer, yaw will drift
*yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.2958f;
}
Core/Src/madgwick.h
+15
View File
@@ -0,0 +1,15 @@
#ifndef MADGWICK_FILTER_H
#define MADGWICK_FILTER_H
typedef struct {
float q[4]; // Quaternion: [qw, qx, qy, qz]
float beta; // Filter gain (tunes correction strength)
float sampleFreq; // Sampling frequency in Hz
} MadgwickFilter;
void MadgwickFilter_Init(MadgwickFilter* filter, float sampleFreq, float beta);
void MadgwickFilter_Update(MadgwickFilter* filter, float gx, float gy, float gz,
float ax, float ay, float az);
void MadgwickFilter_GetEulerAngles(MadgwickFilter* filter, float* roll, float* pitch, float* yaw);
#endif /* MADGWICK_FILTER_H */
Core/Src/madgwick_BACKUP_HEADER_ONLY.h
+135
View File
@@ -0,0 +1,135 @@
#ifndef MADGWICK_H
#define MADGWICK_H
typedef struct {
float q[4]; // Quaternion: [qw, qx, qy, qz]
float beta; // Filter gain (tunes correction strength)
float sampleFreq; // Sampling frequency in Hz
} MadgwickFilter;
void MadgwickFilter_Init(MadgwickFilter* filter, float sampleFreq, float beta);
void MadgwickFilter_Update(MadgwickFilter* filter, float gx, float gy, float gz,
float ax, float ay, float az);
void MadgwickFilter_GetEulerAngles(MadgwickFilter* filter, float* roll, float* pitch, float* yaw);
#ifdef MADGWICK_IMPLEMENTATION
#include <math.h>
// Initialize the filter
void MadgwickFilter_Init(MadgwickFilter* filter, float sampleFreq, float beta) {
filter->q[0] = 1.0f; // qw (scalar component)
filter->q[1] = 0.0f; // qx
filter->q[2] = 0.0f; // qy
filter->q[3] = 0.0f; // qz
filter->beta = beta; // Filter gain (e.g., 0.1)
filter->sampleFreq = sampleFreq; // Sampling frequency in Hz
}
// Update the quaternion using accelerometer and gyroscope data
void MadgwickFilter_Update(MadgwickFilter* filter, float gx, float gy, float gz,
float ax, float ay, float az) {
float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
float recipNorm;
float s[4];
float qDot[4];
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Convert gyroscope data from degrees/s to radians/s
gx *= 0.0174533f; // deg/s to rad/s
gy *= 0.0174533f;
gz *= 0.0174533f;
// Rate of change of quaternion from gyroscope
qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * gx);
// Compute accelerometer objective function and Jacobian
if ((ax != 0.0f) || (ay != 0.0f) || (az != 0.0f)) {
// Normalize accelerometer measurement
recipNorm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated calculations
_2q0 = 2.0f * q[0];
_2q1 = 2.0f * q[1];
_2q2 = 2.0f * q[2];
_2q3 = 2.0f * q[3];
_4q0 = 4.0f * q[0];
_4q1 = 4.0f * q[1];
_4q2 = 4.0f * q[2];
_8q1 = 8.0f * q[1];
_8q2 = 8.0f * q[2];
q0q0 = q[0] * q[0];
q1q1 = q[1] * q[1];
q2q2 = q[2] * q[2];
q3q3 = q[3] * q[3];
// Gradient descent algorithm corrective step
s[0] = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s[1] = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s[2] = 4.0f * q0q0 * q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s[3] = 4.0f * q1q1 * q[3] - _2q1 * ax + 4.0f * q2q2 * q[3] - _2q2 * ay;
// Normalize step magnitude
recipNorm = 1.0f / sqrtf(s[0] * s[0] + s[1] * s[1] + s[2] * s[2] + s[3] * s[3]);
s[0] *= recipNorm;
s[1] *= recipNorm;
s[2] *= recipNorm;
s[3] *= recipNorm;
// Apply feedback step
qDot[0] -= filter->beta * s[0];
qDot[1] -= filter->beta * s[1];
qDot[2] -= filter->beta * s[2];
qDot[3] -= filter->beta * s[3];
}
// Integrate rate of change of quaternion
float dt = 1.0f / filter->sampleFreq;
q[0] += qDot[0] * dt;
q[1] += qDot[1] * dt;
q[2] += qDot[2] * dt;
q[3] += qDot[3] * dt;
// Normalize quaternion
recipNorm = 1.0f / sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
// Update filter state
filter->q[0] = q[0];
filter->q[1] = q[1];
filter->q[2] = q[2];
filter->q[3] = q[3];
}
// Convert quaternion to Euler angles (roll, pitch, yaw) in degrees
void MadgwickFilter_GetEulerAngles(MadgwickFilter* filter, float* roll, float* pitch, float* yaw) {
float q0 = filter->q[0];
float q1 = filter->q[1];
float q2 = filter->q[2];
float q3 = filter->q[3];
// Roll (x-axis rotation)
*roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.2958f;
// Pitch (y-axis rotation)
float sinp = 2.0f * (q0 * q2 - q3 * q1);
if (fabsf(sinp) >= 1)
*pitch = copysignf(M_PI / 2, sinp) * 57.2958f; // Use 90 degrees if out of range
else
*pitch = asinf(sinp) * 57.2958f;
// Yaw (z-axis rotation) - Note: Without magnetometer, yaw will drift
*yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 57.2958f;
}
#endif // end of #ifdef MADGWICK_IMPLEMENTATION
#endif //end of #ifndef MADGWICK_H
Core/Src/main.c
+968
View File
@@ -0,0 +1,968 @@
/* 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.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os2.h"
#include "FreeRTOS.h"
#include "usb_device.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
//#include "madgwick.h"
#include "MadgwickAHRS.h"
#include "sensors.h"
#include "bmm350.h"
#include "bmm350_defs.h"
#include "bmm350_oor.h"
// CDC functionality
#include "usbd_cdc_if.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
DMA_HandleTypeDef hdma_i2c1_tx;
DMA_HandleTypeDef hdma_i2c1_rx;
IPCC_HandleTypeDef hipcc;
RTC_HandleTypeDef hrtc;
SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;
DMA_HandleTypeDef hdma_spi1_tx;
DMA_HandleTypeDef hdma_spi1_rx;
TIM_HandleTypeDef htim2;
/* Definitions for Task_DataAnalys */
osThreadId_t Task_DataAnalysHandle;
const osThreadAttr_t Task_DataAnalys_attributes = {
.name = "Task_DataAnalys",
.priority = (osPriority_t) osPriorityNormal,
.stack_size = 512 * 4
};
/* Definitions for Task_ICM45686 */
osThreadId_t Task_ICM45686Handle;
const osThreadAttr_t Task_ICM45686_attributes = {
.name = "Task_ICM45686",
.priority = (osPriority_t) osPriorityNormal,
.stack_size = 512 * 4
};
/* Definitions for Task_BMM350 */
osThreadId_t Task_BMM350Handle;
const osThreadAttr_t Task_BMM350_attributes = {
.name = "Task_BMM350",
.priority = (osPriority_t) osPriorityNormal,
.stack_size = 512 * 4
};
/* Definitions for Q_ICM */
osMessageQueueId_t Q_ICMHandle;
const osMessageQueueAttr_t Q_ICM_attributes = {
.name = "Q_ICM"
};
/* Definitions for Q_BMM */
osMessageQueueId_t Q_BMMHandle;
const osMessageQueueAttr_t Q_BMM_attributes = {
.name = "Q_BMM"
};
/* USER CODE BEGIN PV */
//struct bmm350_dev dev = {0x00};
//MadgwickFilter IMUQuat;
MadgwickAHRS_Filter IMUQuat;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void PeriphCommonClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_I2C1_Init(void);
static void MX_SPI1_Init(void);
static void MX_SPI2_Init(void);
static void MX_TIM2_Init(void);
static void MX_IPCC_Init(void);
static void MX_RTC_Init(void);
static void MX_RF_Init(void);
void Start_DataAnalysis(void *argument);
void Start_ICM45686(void *argument);
void Start_BMM350(void *argument);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
// implementation of printf() functionality through ITM
int _write(int file, char *prt, int len) {
for (int i = 0; i < len; i++) {
ITM_SendChar(*prt++);
}
return len;
}
/* 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();
/* Config code for STM32_WPAN (HSE Tuning must be done before system clock configuration) */
MX_APPE_Config();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* Configure the peripherals common clocks */
PeriphCommonClock_Config();
/* IPCC initialisation */
MX_IPCC_Init();
/* USER CODE BEGIN SysInit */
// Initialize DWT for the BMM350 delay function to work properly
bmm_init_DWT();
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_I2C1_Init();
MX_SPI1_Init();
MX_SPI2_Init();
MX_TIM2_Init();
MX_RTC_Init();
MX_RF_Init();
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/* Init scheduler */
osKernelInitialize();
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */
/* Create the queue(s) */
/* creation of Q_ICM */
Q_ICMHandle = osMessageQueueNew (1, sizeof(ICM45686_Data), &Q_ICM_attributes);
/* creation of Q_BMM */
Q_BMMHandle = osMessageQueueNew (1, sizeof(struct bmm350_mag_temp_data), &Q_BMM_attributes);
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* creation of Task_DataAnalys */
Task_DataAnalysHandle = osThreadNew(Start_DataAnalysis, NULL, &Task_DataAnalys_attributes);
/* creation of Task_ICM45686 */
Task_ICM45686Handle = osThreadNew(Start_ICM45686, NULL, &Task_ICM45686_attributes);
/* creation of Task_BMM350 */
Task_BMM350Handle = osThreadNew(Start_BMM350, NULL, &Task_BMM350_attributes);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */
/* USER CODE END RTOS_EVENTS */
while(LL_HSEM_1StepLock(HSEM, CFG_HW_CLK48_CONFIG_SEMID));
/* Init code for STM32_WPAN */
MX_APPE_Init();
/* Start scheduler */
osKernelStart();
/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure LSE Drive Capability
*/
HAL_PWR_EnableBkUpAccess();
__HAL_RCC_LSEDRIVE_CONFIG(RCC_LSEDRIVE_MEDIUMHIGH);
/** Configure the main internal regulator output voltage
*/
__HAL_PWR_VOLTAGESCALING_CONFIG(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_OSCILLATORTYPE_LSI1
|RCC_OSCILLATORTYPE_HSE|RCC_OSCILLATORTYPE_LSE
|RCC_OSCILLATORTYPE_MSI;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.LSEState = RCC_LSE_ON;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.MSIState = RCC_MSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.MSICalibrationValue = RCC_MSICALIBRATION_DEFAULT;
RCC_OscInitStruct.MSIClockRange = RCC_MSIRANGE_6;
RCC_OscInitStruct.LSIState = RCC_LSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_MSI;
RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
RCC_OscInitStruct.PLL.PLLN = 32;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV2;
RCC_OscInitStruct.PLL.PLLQ = RCC_PLLQ_DIV2;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Configure the SYSCLKSource, HCLK, PCLK1 and PCLK2 clocks dividers
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK4|RCC_CLOCKTYPE_HCLK2
|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.AHBCLK2Divider = RCC_SYSCLK_DIV2;
RCC_ClkInitStruct.AHBCLK4Divider = RCC_SYSCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
{
Error_Handler();
}
/** Enable MSI Auto calibration
*/
HAL_RCCEx_EnableMSIPLLMode();
}
/**
* @brief Peripherals Common Clock Configuration
* @retval None
*/
void PeriphCommonClock_Config(void)
{
RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
/** Initializes the peripherals clock
*/
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SMPS|RCC_PERIPHCLK_RFWAKEUP;
PeriphClkInitStruct.RFWakeUpClockSelection = RCC_RFWKPCLKSOURCE_HSE_DIV1024;
PeriphClkInitStruct.SmpsClockSelection = RCC_SMPSCLKSOURCE_HSI;
PeriphClkInitStruct.SmpsDivSelection = RCC_SMPSCLKDIV_RANGE1;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN Smps */
/* USER CODE END Smps */
}
/**
* @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 = 0x10B17DB5;
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_ENABLE) != 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 IPCC Initialization Function
* @param None
* @retval None
*/
static void MX_IPCC_Init(void)
{
/* USER CODE BEGIN IPCC_Init 0 */
/* USER CODE END IPCC_Init 0 */
/* USER CODE BEGIN IPCC_Init 1 */
/* USER CODE END IPCC_Init 1 */
hipcc.Instance = IPCC;
if (HAL_IPCC_Init(&hipcc) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN IPCC_Init 2 */
/* USER CODE END IPCC_Init 2 */
}
/**
* @brief RF Initialization Function
* @param None
* @retval None
*/
static void MX_RF_Init(void)
{
/* USER CODE BEGIN RF_Init 0 */
/* USER CODE END RF_Init 0 */
/* USER CODE BEGIN RF_Init 1 */
/* USER CODE END RF_Init 1 */
/* USER CODE BEGIN RF_Init 2 */
/* USER CODE END RF_Init 2 */
}
/**
* @brief RTC Initialization Function
* @param None
* @retval None
*/
static void MX_RTC_Init(void)
{
/* USER CODE BEGIN RTC_Init 0 */
/* USER CODE END RTC_Init 0 */
/* USER CODE BEGIN RTC_Init 1 */
/* USER CODE END RTC_Init 1 */
/** Initialize RTC Only
*/
hrtc.Instance = RTC;
hrtc.Init.HourFormat = RTC_HOURFORMAT_24;
hrtc.Init.AsynchPrediv = CFG_RTC_ASYNCH_PRESCALER;
hrtc.Init.SynchPrediv = CFG_RTC_SYNCH_PRESCALER;
hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;
hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;
hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;
hrtc.Init.OutPutRemap = RTC_OUTPUT_REMAP_NONE;
if (HAL_RTC_Init(&hrtc) != HAL_OK)
{
Error_Handler();
}
/** Enable the WakeUp
*/
if (HAL_RTCEx_SetWakeUpTimer_IT(&hrtc, 0, RTC_WAKEUPCLOCK_RTCCLK_DIV16) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN RTC_Init 2 */
/* USER CODE END RTC_Init 2 */
}
/**
* @brief SPI1 Initialization Function
* @param None
* @retval None
*/
static void MX_SPI1_Init(void)
{
/* USER CODE BEGIN SPI1_Init 0 */
/* USER CODE END SPI1_Init 0 */
/* USER CODE BEGIN SPI1_Init 1 */
/* USER CODE END SPI1_Init 1 */
/* SPI1 parameter configuration*/
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_MASTER;
hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi1.Init.CRCPolynomial = 7;
hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
hspi1.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
if (HAL_SPI_Init(&hspi1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN SPI1_Init 2 */
/* USER CODE END SPI1_Init 2 */
}
/**
* @brief SPI2 Initialization Function
* @param None
* @retval None
*/
static void MX_SPI2_Init(void)
{
/* USER CODE BEGIN SPI2_Init 0 */
/* USER CODE END SPI2_Init 0 */
/* USER CODE BEGIN SPI2_Init 1 */
/* USER CODE END SPI2_Init 1 */
/* SPI2 parameter configuration*/
hspi2.Instance = SPI2;
hspi2.Init.Mode = SPI_MODE_MASTER;
hspi2.Init.Direction = SPI_DIRECTION_2LINES;
hspi2.Init.DataSize = SPI_DATASIZE_4BIT;
hspi2.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi2.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi2.Init.NSS = SPI_NSS_SOFT;
hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi2.Init.CRCPolynomial = 7;
hspi2.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
hspi2.Init.NSSPMode = SPI_NSS_PULSE_ENABLE;
if (HAL_SPI_Init(&hspi2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN SPI2_Init 2 */
/* USER CODE END SPI2_Init 2 */
}
/**
* @brief TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2;
htim2.Init.Prescaler = 63;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 999;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_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();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/**
* Enable DMA controller clock
*/
static void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMAMUX1_CLK_ENABLE();
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Channel1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel1_IRQn);
/* DMA1_Channel2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
/* DMA1_Channel3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
/* DMA1_Channel4_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel4_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel4_IRQn);
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIO_SPI1_IMU_CS_GPIO_Port, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, GPIO_SPI2_SD_CD_Pin|GPIO_SPI2_SD_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIO_SD_LDO_EN_GPIO_Port, GPIO_SD_LDO_EN_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : GPIO_SPI1_IMU_CS_Pin */
GPIO_InitStruct.Pin = GPIO_SPI1_IMU_CS_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
HAL_GPIO_Init(GPIO_SPI1_IMU_CS_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : GPIO_SPI2_SD_CD_Pin GPIO_SPI2_SD_CS_Pin GPIO_SD_LDO_EN_Pin */
GPIO_InitStruct.Pin = GPIO_SPI2_SD_CD_Pin|GPIO_SPI2_SD_CS_Pin|GPIO_SD_LDO_EN_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pin : GPIO_IMU_INT_Pin */
GPIO_InitStruct.Pin = GPIO_IMU_INT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIO_IMU_INT_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : GPIO_MAGN_INT_Pin */
GPIO_InitStruct.Pin = GPIO_MAGN_INT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIO_MAGN_INT_GPIO_Port, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/* USER CODE BEGIN Header_Start_DataAnalysis */
/**
* @brief Function implementing the Task_DataAnalys thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_DataAnalysis */
void Start_DataAnalysis(void *argument)
{
/* init code for USB_Device */
MX_USB_Device_Init();
/* USER CODE BEGIN 5 */
// HAL_StatusTypeDef status;
// char output_buffer[128];
ICM45686_Data imu_data;
struct bmm350_mag_temp_data magn_data;
// struct bmm350_raw_mag_data magn_data;
QuaternionData Quat_Dat;
Quat_Dat.StartByte = PACKET_START_BYTE;
Quat_Dat.SensorAddress = BMM350_I2C_ADSEL_SET_LOW;
Quat_Dat.EndByte = PACKET_END_BYTE;
MadgwickAHRS_init(&IMUQuat, 6400.0f, 0.15f);
/* Infinite loop */
for(;;)
{
osMessageQueueGet(Q_BMMHandle, &magn_data, NULL, osWaitForever);
osMessageQueueGet(Q_ICMHandle, &imu_data, NULL, osWaitForever);
// MadgwickAHRS_update(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
// imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2], // ax, ay, yz
// magn_data.x, magn_data.y, magn_data.z); // mx, my, mz
MadgwickAHRS_update_IMU(&IMUQuat, imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], //gx, gy, gz
imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2]); // ax, ay, yz)
Quat_Dat.qw = IMUQuat.q[0];
Quat_Dat.qx = IMUQuat.q[1];
Quat_Dat.qy = IMUQuat.q[2];
Quat_Dat.qz = IMUQuat.q[3];
CDC_Transmit_FS((uint8_t*)&Quat_Dat, sizeof(Quat_Dat));
// DEBUG
// sprintf(output_buffer, "------------\n");
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
//
// sprintf(output_buffer, "IMUQuat:\tqw: %.2f, qx: %.2f, qy: %.2f, qz: %.2f\n",
// IMUQuat.q[0], IMUQuat.q[1], IMUQuat.q[2], IMUQuat.q[3]);
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
// sprintf(output_buffer, "ICM:\ta_x: %.2f a_y: %.2f a_z: %.2f\t g_x: %.2f g_y: %.2f g_z: %.2f temp: %.2f \n", imu_data.processed_imu_data[0], imu_data.processed_imu_data[1], imu_data.processed_imu_data[2],
// imu_data.processed_imu_data[3], imu_data.processed_imu_data[4], imu_data.processed_imu_data[5], imu_data.processed_imu_data[6]);
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
//
// // compensated mag data
// sprintf(output_buffer, "BMM:\tX: %.2f uT, Y: %.2f uT, Z: %.2f uT, Temp: %.2f C\n",
// magn_data.x, magn_data.y, magn_data.z, magn_data.temperature);
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
//// // raw mag data
//// sprintf(output_buffer, "BMM:\tX: %.2f uT, Y: %.2f uT, Z: %.2f uT, Temp: %.2f C\n",
//// magn_data.raw_xdata, magn_data.raw_ydata, magn_data.raw_zdata, magn_data.raw_data_t);
//// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
//
// sprintf(output_buffer, "Quaternion:\tqw: %.2f, qx: %.2f, qy: %.2f, qz: %.2f\n",
// Quat_Dat.qw, Quat_Dat.qx, Quat_Dat.qy, Quat_Dat.qz);
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
//
// sprintf(output_buffer, "------------\n");
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
osDelay(100);
}
/* USER CODE END 5 */
}
/* USER CODE BEGIN Header_Start_ICM45686 */
/**
* @brief Function implementing the Task_ICM45686 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_ICM45686 */
void Start_ICM45686(void *argument)
{
/* USER CODE BEGIN Start_ICM45686 */
HAL_StatusTypeDef status;
// char output_buffer[128];
xIMU_Task = osThreadGetId();
// ICM
//uint8_t usb_response;
uint8_t raw_imu_data[15];
int16_t imu_sensor_data[7];
ICM45686_Data processed_data;
ICM45686_HandleTypeDef imu;
imu.hspi = &hspi1;
imu.GPIO_Port = GPIO_SPI1_IMU_CS_GPIO_Port;
imu.GPIO_Pin = GPIO_SPI1_IMU_CS_Pin;
imu.acc_fs = ICM45686_ACC_FS_4G;
imu.acc_ssf = ICM45686_ACC_SSF[ICM45686_ACC_FS_4G >> 4];
imu.gyro_fs = ICM45686_GYRO_FS_250DPS;
imu.gyro_ssf = ICM45686_GYRO_SSF[ICM45686_GYRO_FS_250DPS >> 4];
imu.odr = ICM45686_ODR_6_4kHz_LN ;
uint8_t tx[15] = {0x00};
tx[0] = 0x00 | 0x80;
init_icm(&imu);
// calibrate_icm(&imu, 1000);
/* Infinite loop */
for(;;)
{
// imu
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive_DMA(&hspi1, tx, raw_imu_data, sizeof(raw_imu_data)); //(&hspi1, tx, raw_imu_data, sizeof(raw_imu_data), 50);
status = read_icm_dma(&imu, tx, raw_imu_data);
if (status != HAL_OK) {
printf("Reading IMU sensordata failed: %d\n", status);
}
imu_sensor_data[0] = (int16_t)(raw_imu_data[1] << 8 | raw_imu_data[2]); // a_x
imu_sensor_data[1] = (int16_t)(raw_imu_data[3] << 8 | raw_imu_data[4]); // a_y
imu_sensor_data[2] = (int16_t)(raw_imu_data[5] << 8 | raw_imu_data[6]); // a_z
imu_sensor_data[3] = (int16_t)(raw_imu_data[7] << 8 | raw_imu_data[8]); // g_x
imu_sensor_data[4] = (int16_t)(raw_imu_data[9] << 8 | raw_imu_data[10]); // g_y
imu_sensor_data[5] = (int16_t)(raw_imu_data[11] << 8 | raw_imu_data[12]); // g_z
imu_sensor_data[6] = (int16_t)(raw_imu_data[13] << 8 | raw_imu_data[14]); // t
processed_data.processed_imu_data[0] = (float)imu_sensor_data[0] / (float)imu.acc_fs;
processed_data.processed_imu_data[1] = (float)imu_sensor_data[1] / (float)imu.acc_fs;
processed_data.processed_imu_data[2] = (float)imu_sensor_data[2] / (float)imu.acc_fs;
processed_data.processed_imu_data[3] = (float)imu_sensor_data[3] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[4] = (float)imu_sensor_data[4] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[5] = (float)imu_sensor_data[5] / (float)imu.gyro_ssf;
processed_data.processed_imu_data[6] = (float)(imu_sensor_data[6] / 128.0 + 25.0);
if (imu.calibated) {
processed_data.processed_imu_data[0] -= imu.imu_bias[0];
processed_data.processed_imu_data[1] -= imu.imu_bias[1];
processed_data.processed_imu_data[2] -= imu.imu_bias[2];
processed_data.processed_imu_data[3] -= imu.imu_bias[3];
processed_data.processed_imu_data[4] -= imu.imu_bias[4];
processed_data.processed_imu_data[5] -= imu.imu_bias[5];
processed_data.processed_imu_data[6] -= imu.imu_bias[7];
}
// printf("[0]: %.2f [3]: %.2f [6]: %.2f\n", processed_data.processed_imu_data[0], processed_data.processed_imu_data[3], processed_data.processed_imu_data[6]);
osMessageQueuePut(Q_ICMHandle, &processed_data, 0, 5);
osDelay(100);
}
/* USER CODE END Start_ICM45686 */
}
/* USER CODE BEGIN Header_Start_BMM350 */
/**
* @brief Function implementing the Task_BMM350 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Start_BMM350 */
void Start_BMM350(void *argument)
{
/* USER CODE BEGIN Start_BMM350 */
// HAL_StatusTypeDef status;
// char output_buffer[128];
xMagn_Task = osThreadGetId();
// BMM
struct bmm350_dev dev = {0x00};
int8_t rslt;
struct bmm350_mag_temp_data mag_data;
osDelay(250);
// struct bmm350_raw_mag_data mag_data;
// struct bmm350_mag_temp_data mag_temp_data;
// struct bmm350_pmu_cmd_status_0 pmu_cmd_stat_0;
// ODR: 200HZ, AVG:
rslt = init_bmm(&dev);
if (rslt != BMM350_OK) {
printf("BMM350 initialization failed: %d\n", rslt);
printf("Chip id: 0x%02X\n", dev.chip_id);
}
/* Infinite loop */
for(;;)
{
// magnetometer
rslt = bmm350_get_compensated_mag_xyz_temp_data(&mag_data, &dev);
// rslt = bmm350_read_uncomp_mag_temp_data(&mag_data, &dev);
if (rslt == BMM350_OK)
{
// sprintf(output_buffer, "X: %.2f uT, Y: %.2f uT, Z: %.2f uT, Temp: %.2f C\n",
// mag_data.x, mag_data.y, mag_data.z, mag_data.temperature);
// CDC_Transmit_FS((uint8_t*)output_buffer, (uint16_t)strlen(output_buffer));
osMessageQueuePut(Q_BMMHandle, &mag_data, 0, 5);
}
osDelay(100);
}
/* USER CODE END Start_BMM350 */
}
/**
* @brief Period elapsed callback in non blocking mode
* @note This function is called when TIM16 interrupt took place, inside
* HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
* a global variable "uwTick" used as application time base.
* @param htim : TIM handle
* @retval None
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
/* USER CODE BEGIN Callback 0 */
/* USER CODE END Callback 0 */
if (htim->Instance == TIM16)
{
HAL_IncTick();
}
/* USER CODE BEGIN Callback 1 */
/* USER CODE END Callback 1 */
}
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* 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 */
Core/Src/sensors.c
+305
View File
@@ -0,0 +1,305 @@
#include "main.h"
#include "sensors.h"
//#include "stm32wbxx_hal_i2c.h"
#include <stdio.h>
#include "cmsis_os2.h"
#include "task.h"
#define HAL_I2C_TIMEOUT 100
//extern osThreadId_t defaultTaskHandle;
//TaskHandle_t xStartDefaultTask = NULL;
//extern osThreadId_t defaultTaskHandle
osThreadId_t xIMU_Task = NULL;
osThreadId_t xMagn_Task = NULL;
//osThreadId_t xData_Task = NULL;
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
if (hspi->Instance == SPI1) {
HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
// notify the task that the data is ready
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address) {
// printf("HAL_I2C_MemRxCpltCallback");
if (I2C_address->Instance == I2C1) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
// printf("HAL_I2C_MemTxCpltCallback");
if (I2C_address->Instance == I2C1) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
if (hi2c->Instance == I2C1) {
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
printf("ErrorCallback: %ld", HAL_I2C_GetError(hi2c));
vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}
}
// ICM 45686
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]) {
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_SET);
return status;
}
HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]) {
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
HAL_StatusTypeDef status = HAL_SPI_TransmitReceive_DMA(imu->hspi, tx, rx, sizeof(rx));
return status;
}
HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[]) {
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
HAL_StatusTypeDef status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_SET);
return status;
}
void init_icm(ICM45686_HandleTypeDef *imu) {
// TODO: Add settings as enum to choose from
// FIFO
// Interrupt
// APEX???
// EDMP???
uint8_t tx[2], rx[2];
HAL_StatusTypeDef status;
printf("Initializing ICM45686\n");
// WHO AM
tx[0] = ICM45686_WHO_AM_I | 0x80;
// HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx);
if (status != HAL_OK) {
printf("Error in reading WHO AM I:%d\n", status);
} else {
printf("WHO AM I (0xE9): 0x%02X \n", rx[1]);
}
// acc
tx[0] = ICM45686_ACCEL_CONFIG0;
tx[1] = imu->acc_fs | imu->odr;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = write_icm(imu, tx);
if (status != HAL_OK) {
printf("Error in setting ACCEL_CONFIG0:%d\n", status);
} else {
printf("ACCEL_CONFIG0 wrote: 0x%02X \n", tx[1]);
}
tx[0] = ICM45686_ACCEL_CONFIG0 | 0x80;
tx[1] = 0x00;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx);
if (status != HAL_OK) {
printf("Error reading ACCEL_CONFIG0:%d\n", status);
} else {
printf("ACCEL_CONFIG0 read: 0x%02X \n", rx[1]);
}
// gyro
tx[0] = ICM45686_GYRO_CONFIG0;
tx[1] = imu->gyro_fs | imu->odr;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = write_icm(imu, tx);
if (status != HAL_OK) {
printf("Error in setting GYRO_CONFIG0:%d\n", status);
} else {
printf("GYRO_CONFIG0 wrote: 0x%02X \n", tx[1]);
}
tx[0] = ICM45686_GYRO_CONFIG0 | 0x80;
tx[1] = 0x00;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx);
if (status != HAL_OK) {
printf("Error reading GYRO_CONFIG0:%d\n", status);
} else {
printf("GYRO_CONFIG0 read: 0x%02X \n", rx[1]);
}
// power settings
tx[0] = ICM45686_PWR_MGMT0;
tx[1] = 0x0F; //0x10 | 0x03;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = write_icm(imu, tx);
if (status != HAL_OK) {
printf("Error in setting PWR_MGMT0:%d\n", status);
} else {
printf("PWR_MGMT0 wrote: 0x%02X \n", tx[1]);
}
tx[0] = ICM45686_PWR_MGMT0 | 0x80;
tx[1] = 0x00;
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
// status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
// HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
status = read_icm(imu, tx, rx);
if (status != HAL_OK) {
printf("Error reading PWR_MGMT0:%d\n", status);
} else {
printf("PWR_MGMT0 read: 0x%02X \n", rx[1]);
}
osDelay(250);
}
void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
HAL_StatusTypeDef status;
uint8_t raw_data[15];
int16_t sensor_data[7] = {0};
uint8_t tx[15] = {0x00};
tx[0] = 0x00 | 0x80;
for (int i = 1; i <= samples; i++) {
status = read_icm_dma(imu, tx, raw_data);
if (status != HAL_OK) {
printf("Calibration read ICM failed: %d\n", status);
}
sensor_data[0] += (int16_t)(raw_data[1] << 8 | raw_data[2]); // a_x
sensor_data[1] += (int16_t)(raw_data[3] << 8 | raw_data[4]); // a_y
sensor_data[2] += (int16_t)(raw_data[5] << 8 | raw_data[6]); // a_z
sensor_data[3] += (int16_t)(raw_data[7] << 8 | raw_data[8]); // g_x
sensor_data[4] += (int16_t)(raw_data[9] << 8 | raw_data[10]); // g_y
sensor_data[5] += (int16_t)(raw_data[11] << 8 | raw_data[12]); // g_z
sensor_data[6] += (int16_t)(raw_data[13] << 8 | raw_data[14]); // t
}
imu->imu_bias[0] = (float)sensor_data[0] / (float)samples;
imu->imu_bias[1] = (float)sensor_data[1] / (float)samples;
imu->imu_bias[2] = (float)sensor_data[2] / (float)samples;
imu->imu_bias[3] = (float)sensor_data[3] / (float)samples;
imu->imu_bias[4] = (float)sensor_data[4] / (float)samples;
imu->imu_bias[5] = (float)sensor_data[5] / (float)samples;
imu->imu_bias[6] = (float)sensor_data[6] / (float)samples;
// remrove gravity
imu->imu_bias[2] -= 1.0f;
imu->calibated = true;
printf("Calibration finished of ICM45686\n");
printf("a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
imu->imu_bias[0], imu->imu_bias[1], imu->imu_bias[2],
imu->imu_bias[3], imu->imu_bias[4], imu->imu_bias[5],
imu->imu_bias[6]);
}
// BMM350
void bmm_init_DWT(void){
// Enable trace and debug block (TRCENA)
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
// Reset cycle counter
DWT->CYCCNT = 0;
// Enable the cycle counter
DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}
BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr){
// Extract I2C device address from intf_ptr
uint8_t device_addr = *(uint8_t*)intf_ptr;
// STM32 HAL: Write register address, then read data
// Device address is left-shifted by 1 for STM32 HAL
// if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
if (HAL_I2C_Mem_Read_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length) == HAL_OK) {
if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
return BMM350_INTF_RET_SUCCESS; // 0
}
}
return -1; // Communication failure
}
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
uint8_t device_addr = *(uint8_t*)intf_ptr;
// uint8_t rslt = HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length);
// printf("rslt write: %d\n", rslt);
// // STM32 HAL: Write register address + data
if (HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length) == HAL_OK) {
// if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
// if (rslt == HAL_OK) {
if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
return BMM350_INTF_RET_SUCCESS; // 0
}
}
return -1;
}
void bmm350_delay(uint32_t period_us, void *intf_ptr){
(void)intf_ptr; // Unused
// For STM32, use DWT cycle counter for accurate microsecond delays
uint32_t start = DWT->CYCCNT;
uint32_t cycles = period_us * (SystemCoreClock / 1000000);
while ((DWT->CYCCNT - start) < cycles);
}
// Initialization function
int8_t init_bmm(struct bmm350_dev *dev) {
int8_t rslt;
// Static variable to hold I2C device address
// Must be static or global because dev.intf_ptr will point to it
static uint8_t dev_addr = BMM350_I2C_ADSEL_SET_LOW;
// Assign function pointers
dev->read = bmm350_i2c_read;
dev->write = bmm350_i2c_write;
dev->delay_us = bmm350_delay;
dev->intf_ptr = &dev_addr;
printf("Starting up and configuring BMM350\n");
// Initialize BMM350 driver
rslt = bmm350_init(dev);
if (rslt != BMM350_OK)
{
// Handle error: chip ID mismatch, communication failure, etc.
return rslt;
}
printf("Initialization: %d\n", rslt);
printf("CHIP_ID (should be 0x33) - Read Register: 0x00 BMM350 Chip ID: 0x%X\n", dev->chip_id);
// Set powermode, odr and avg
rslt = bmm350_set_powermode(BMM350_NORMAL_MODE, dev);
printf("Powermode (%d): %d\n", BMM350_NORMAL_MODE, rslt);
rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, dev);
printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, rslt);
return BMM350_OK;
}
Core/Src/sensors.h
+164
View File
@@ -0,0 +1,164 @@
#include "main.h"
#include <stdbool.h>
#include "bmm350.h"
#include "bmm350_defs.h"
#include "bmm350_oor.h"
#include "FreeRTOS.h"
#include "cmsis_os2.h"
// I2C Handle
extern I2C_HandleTypeDef hi2c1;
extern osThreadId_t defaultTaskHandle;
extern osThreadId_t xIMU_Task;
extern osThreadId_t xMagn_Task;
//extern osThreadId_t xData_Task;
// ICM45686
typedef struct {
SPI_HandleTypeDef *hspi;
GPIO_TypeDef *GPIO_Port;
uint16_t GPIO_Pin;
uint8_t acc_fs;
uint8_t gyro_fs;
uint8_t odr;
float acc_ssf;
float gyro_ssf;
bool calibated;
// float accel_bias[3];
// float gyro_bias[3];
float imu_bias[7];
} ICM45686_HandleTypeDef;
typedef struct {
float processed_imu_data[7];
} ICM45686_Data;
// dataframe for sending quaternions over usb
typedef struct __attribute__((packed)) {
uint8_t StartByte;
uint8_t SensorAddress;
float qw;
float qx;
float qy;
float qz;
uint8_t EndByte;
} QuaternionData;
// enum to hold the packet dataframe data
typedef enum {
PACKET_START_BYTE = 0xDE,
PACKET_END_BYTE = 0xAD
} Packet_Bytes;
typedef enum {
ICM45686_WHO_AM_I = 0x72, // 0x72 = 01110010, dann den wert NIX(???)) einfach nur lesen
ICM45686_ACCEL_CONFIG0 = 0x1B,
ICM45686_GYRO_CONFIG0 = 0x1C,
ICM45686_FIFO_CONFIG0 = 0x1D,
ICM45686_PWR_MGMT0 = 0x10,
ICM45686_ODR_DECIMATE_CONFIG = 0x40,
ICM45686_ACCEL_DATA = 0x00,
ICM45686_GYRO_DATA = 0x06,
ICM45686_FIFO_COUNT = 0x12,
ICM45686_FIFO_DATA = 0x14
} ICM45686_registers;
typedef enum {
ICM45686_ACC_FS_32G = 0x00,
ICM45686_ACC_FS_16G = 0x10,
ICM45686_ACC_FS_8G = 0x20,
ICM45686_ACC_FS_4G = 0x30,
ICM45686_ACC_FS_2G = 0x40,
} ICM45686_ACC_CONF;
//typedef enum {
// ICM45686_ACC_SSF_32G = 16384.0,
// ICM45686_ACC_SSF_16G = 8192.0,
// ICM45686_ACC_SSF_8G = 4096.0,
// ICM45686_ACC_SSF_4G = 2048.0,
// ICM45686_ACC_SSF_2G = 1024.0,
//} ICM45686_ACC_FACTORS;
static const float ICM45686_ACC_SSF [5] = {
1024.0f, 2048.0f, 4096.0f, 8192.0f, 16384.0f
};
typedef enum {
ICM45686_GYRO_FS_4000DPS = 0x00,
ICM45686_GYRO_FS_2000DPS = 0x10,
ICM45686_GYRO_FS_1000DPS = 0x20,
ICM45686_GYRO_FS_500DPS = 0x30,
ICM45686_GYRO_FS_250DPS = 0x40,
ICM45686_GYRO_FS_125DPS = 0x50,
ICM45686_GYRO_FS_62_5DPS = 0x60,
ICM45686_GYRO_FS_31_25DPS = 0x70,
ICM45686_GYRO_FS_15_625DPS = 0x80,
} ICM45686_GYRO_CONF;
//typedef enum {
// ICM45686_GYRO_SSF_4000DPS = 2097.2,
// ICM45686_GYRO_SSF_2000DPS = 1048.6,
// ICM45686_GYRO_SSF_1000DPS = 524.3,
// ICM45686_GYRO_SSF_500DPS = 262.0,
// ICM45686_GYRO_SSF_250DPS = 131.0,
// ICM45686_GYRO_SSF_125DPS = 65.8,
// ICM45686_GYRO_SSF_62_5DPS = 32.8,
// ICM45686_GYRO_SSF_31_25DPS = 16.4,
// ICM45686_GYRO_SSF_15_626DPS = 8.2,
//} ICM45686_GYRO_FACTORS;
static const float ICM45686_GYRO_SSF [9] = {
8.2f, 16.4f, 32.8f, 65.5f, 131.0f, 262.0f, 524.3f, 1048.6f, 2097.2f
};
typedef enum {
ICM45686_ODR_6_4kHz_LN = 0x03 ,
ICM45686_ODR_3_2kHz_LN = 0x04,
ICM45686_ODR_1_6kHz_LN = 0x05,
ICM45686_ODR_800Hz_LN = 0x06,
ICM45686_ODR_400Hz_LNLP = 0x07,
ICM45686_ODR_200Hz_LNLP = 0x08,
ICM45686_ODR_100Hz_LNLP = 0x09,
ICM45686_ODR_50Hz_LNLP = 0x0A,
ICM45686_ODR_25Hz_LNLP = 0x0B,
ICM45686_ODR_12_5Hz_LNLP = 0x0C,
ICM45686_ODR_6_26Hz_LP = 0x0D,
ICM45686_ODR_3_125Hz_LP = 0x0E,
ICM45686_ODR_1_5625Hz_LP = 0x0F
} ICM45686_ODR;
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi);
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address);
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address);
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c);
// ICM45686
void init_icm(ICM45686_HandleTypeDef *imu);
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]);
HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]);
HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[]);
void calibrate_icm (ICM45686_HandleTypeDef * imu, int samples);
// BMM350 - Declarations of read/write/delay functions for the BMM350 API
void bmm_init_DWT(void);
BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr);
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr);
void bmm350_delay(uint32_t period_us, void *intf_ptr);
int8_t init_bmm(struct bmm350_dev *dev);
// structs that hold the sensor data
typedef struct {
uint8_t raw_imu[14];
uint8_t raw_magn [9];
} raw_sensor_data;
Core/Src/stm32_lpm_if.c
+347
View File
@@ -0,0 +1,347 @@
/* USER CODE BEGIN Header */
/**
***************************************************************************************
* @file stm32_lpm_if.c
* @author MCD Application Team
* @brief Low layer function to enter/exit low power modes (stop, sleep).
***************************************************************************************
* @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 "stm32_lpm_if.h"
#include "stm32_lpm.h"
#include "app_conf.h"
/* USER CODE BEGIN include */
/* USER CODE END include */
/* Exported variables --------------------------------------------------------*/
const struct UTIL_LPM_Driver_s UTIL_PowerDriver =
{
PWR_EnterSleepMode,
PWR_ExitSleepMode,
PWR_EnterStopMode,
PWR_ExitStopMode,
PWR_EnterOffMode,
PWR_ExitOffMode,
};
/* Private function prototypes -----------------------------------------------*/
static void Switch_On_HSI(void);
static void EnterLowPower(void);
static void ExitLowPower(void);
/* USER CODE BEGIN Private_Function_Prototypes */
/* USER CODE END Private_Function_Prototypes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN Private_Typedef */
/* USER CODE END Private_Typedef */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN Private_Define */
/* USER CODE END Private_Define */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN Private_Macro */
/* USER CODE END Private_Macro */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Private_Variables */
/* USER CODE END Private_Variables */
/* Functions Definition ------------------------------------------------------*/
/**
* @brief Enters Low Power Off Mode
* @param none
* @retval none
*/
void PWR_EnterOffMode(void)
{
/* USER CODE BEGIN PWR_EnterOffMode_1 */
/* USER CODE END PWR_EnterOffMode_1 */
/**
* The systick should be disabled for the same reason than when the device enters stop mode because
* at this time, the device may enter either OffMode or StopMode.
*/
HAL_SuspendTick();
EnterLowPower();
/************************************************************************************
* ENTER OFF MODE
***********************************************************************************/
/*
* There is no risk to clear all the WUF here because in the current implementation, this API is called
* in critical section. If an interrupt occurs while in that critical section before that point,
* the flag is set and will be cleared here but the system will not enter Off Mode
* because an interrupt is pending in the NVIC. The ISR will be executed when moving out
* of this critical section
*/
LL_PWR_ClearFlag_WU();
LL_PWR_SetPowerMode(LL_PWR_MODE_STANDBY);
LL_LPM_EnableDeepSleep(); /**< Set SLEEPDEEP bit of Cortex System Control Register */
/**
* This option is used to ensure that store operations are completed
*/
#if defined (__CC_ARM) || defined (__ARMCC_VERSION)
__force_stores();
#endif
__WFI();
/* USER CODE BEGIN PWR_EnterOffMode_2 */
/* USER CODE END PWR_EnterOffMode_2 */
return;
}
/**
* @brief Exits Low Power Off Mode
* @param none
* @retval none
*/
void PWR_ExitOffMode(void)
{
/* USER CODE BEGIN PWR_ExitOffMode_1 */
/* USER CODE END PWR_ExitOffMode_1 */
HAL_ResumeTick();
/* USER CODE BEGIN PWR_ExitOffMode_2 */
/* USER CODE END PWR_ExitOffMode_2 */
return;
}
/**
* @brief Enters Low Power Stop Mode
* @note ARM exists the function when waking up
* @param none
* @retval none
*/
void PWR_EnterStopMode(void)
{
/* USER CODE BEGIN PWR_EnterStopMode_1 */
/* USER CODE END PWR_EnterStopMode_1 */
/**
* When HAL_DBGMCU_EnableDBGStopMode() is called to keep the debugger active in Stop Mode,
* the systick shall be disabled otherwise the cpu may crash when moving out from stop mode
*
* When in production, the HAL_DBGMCU_EnableDBGStopMode() is not called so that the device can reach best power consumption
* However, the systick should be disabled anyway to avoid the case when it is about to expire at the same time the device enters
* stop mode (this will abort the Stop Mode entry).
*/
HAL_SuspendTick();
/**
* This function is called from CRITICAL SECTION
*/
EnterLowPower();
/************************************************************************************
* ENTER STOP MODE
***********************************************************************************/
LL_PWR_SetPowerMode(LL_PWR_MODE_STOP2);
LL_LPM_EnableDeepSleep(); /**< Set SLEEPDEEP bit of Cortex System Control Register */
/**
* This option is used to ensure that store operations are completed
*/
#if defined (__CC_ARM) || defined (__ARMCC_VERSION)
__force_stores();
#endif
__WFI();
/* USER CODE BEGIN PWR_EnterStopMode_2 */
/* USER CODE END PWR_EnterStopMode_2 */
return;
}
/**
* @brief Exits Low Power Stop Mode
* @note Enable the pll at 32MHz
* @param none
* @retval none
*/
void PWR_ExitStopMode(void)
{
/* USER CODE BEGIN PWR_ExitStopMode_1 */
/* USER CODE END PWR_ExitStopMode_1 */
/**
* This function is called from CRITICAL SECTION
*/
ExitLowPower();
HAL_ResumeTick();
/* USER CODE BEGIN PWR_ExitStopMode_2 */
/* USER CODE END PWR_ExitStopMode_2 */
return;
}
/**
* @brief Enters Low Power Sleep Mode
* @note ARM exits the function when waking up
* @param none
* @retval none
*/
void PWR_EnterSleepMode(void)
{
/* USER CODE BEGIN PWR_EnterSleepMode_1 */
/* USER CODE END PWR_EnterSleepMode_1 */
HAL_SuspendTick();
/************************************************************************************
* ENTER SLEEP MODE
***********************************************************************************/
LL_LPM_EnableSleep(); /**< Clear SLEEPDEEP bit of Cortex System Control Register */
/**
* This option is used to ensure that store operations are completed
*/
#if defined (__CC_ARM) || defined (__ARMCC_VERSION)
__force_stores();
#endif
__WFI();
/* USER CODE BEGIN PWR_EnterSleepMode_2 */
/* USER CODE END PWR_EnterSleepMode_2 */
return;
}
/**
* @brief Exits Low Power Sleep Mode
* @note ARM exits the function when waking up
* @param none
* @retval none
*/
void PWR_ExitSleepMode(void)
{
/* USER CODE BEGIN PWR_ExitSleepMode_1 */
/* USER CODE END PWR_ExitSleepMode_1 */
HAL_ResumeTick();
/* USER CODE BEGIN PWR_ExitSleepMode_2 */
/* USER CODE END PWR_ExitSleepMode_2 */
return;
}
/*************************************************************
*
* LOCAL FUNCTIONS
*
*************************************************************/
/**
* @brief Setup the system to enter either stop or off mode
* @param none
* @retval none
*/
static void EnterLowPower(void)
{
/**
* This function is called from CRITICAL SECTION
*/
while(LL_HSEM_1StepLock(HSEM, CFG_HW_RCC_SEMID));
if (! LL_HSEM_1StepLock(HSEM, CFG_HW_ENTRY_STOP_MODE_SEMID))
{
if(LL_PWR_IsActiveFlag_C2DS() || LL_PWR_IsActiveFlag_C2SB())
{
/* Release ENTRY_STOP_MODE semaphore */
LL_HSEM_ReleaseLock(HSEM, CFG_HW_ENTRY_STOP_MODE_SEMID, 0);
Switch_On_HSI();
__HAL_FLASH_SET_LATENCY(FLASH_LATENCY_0);
}
}
else
{
Switch_On_HSI();
__HAL_FLASH_SET_LATENCY(FLASH_LATENCY_0);
}
/* Release RCC semaphore */
LL_HSEM_ReleaseLock(HSEM, CFG_HW_RCC_SEMID, 0);
return;
}
/**
* @brief Restore the system to exit stop mode
* @param none
* @retval none
*/
static void ExitLowPower(void)
{
/* Release ENTRY_STOP_MODE semaphore */
LL_HSEM_ReleaseLock(HSEM, CFG_HW_ENTRY_STOP_MODE_SEMID, 0);
while(LL_HSEM_1StepLock(HSEM, CFG_HW_RCC_SEMID));
if(LL_RCC_GetSysClkSource() == LL_RCC_SYS_CLKSOURCE_STATUS_HSI)
{
/* Restore the clock configuration of the application in this user section */
/* USER CODE BEGIN ExitLowPower_1 */
/* USER CODE END ExitLowPower_1 */
}
else
{
/* If the application is not running on HSE restore the clock configuration in this user section */
/* USER CODE BEGIN ExitLowPower_2 */
/* USER CODE END ExitLowPower_2 */
}
/* Release RCC semaphore */
LL_HSEM_ReleaseLock(HSEM, CFG_HW_RCC_SEMID, 0);
return;
}
/**
* @brief Switch the system clock on HSI
* @param none
* @retval none
*/
static void Switch_On_HSI(void)
{
LL_RCC_HSI_Enable();
while(!LL_RCC_HSI_IsReady());
LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_HSI);
LL_RCC_SetSMPSClockSource(LL_RCC_SMPS_CLKSOURCE_HSI);
while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_HSI);
return;
}
/* USER CODE BEGIN Private_Functions */
/* USER CODE END Private_Functions */
Core/Src/stm32wbxx_hal_msp.c
+584
View File
@@ -0,0 +1,584 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32wbxx_hal_msp.c
* @brief This file provides code for the MSP Initialization
* and de-Initialization codes.
******************************************************************************
* @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"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern DMA_HandleTypeDef hdma_i2c1_tx;
extern DMA_HandleTypeDef hdma_i2c1_rx;
extern DMA_HandleTypeDef hdma_spi1_tx;
extern DMA_HandleTypeDef hdma_spi1_rx;
/* 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_HSEM_CLK_ENABLE();
HAL_PWREx_EnableVddUSB();
/* System interrupt init*/
/* PendSV_IRQn interrupt configuration */
HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0);
/* Peripheral interrupt init */
/* HSEM_IRQn interrupt configuration */
HAL_NVIC_SetPriority(HSEM_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(HSEM_IRQn);
/* 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 PeriphClkInitStruct = {0};
if(hi2c->Instance==I2C1)
{
/* USER CODE BEGIN I2C1_MspInit 0 */
/* USER CODE END I2C1_MspInit 0 */
/** Initializes the peripherals clock
*/
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_I2C1;
PeriphClkInitStruct.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != 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_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* Peripheral clock enable */
__HAL_RCC_I2C1_CLK_ENABLE();
/* I2C1 DMA Init */
/* I2C1_TX Init */
hdma_i2c1_tx.Instance = DMA1_Channel3;
hdma_i2c1_tx.Init.Request = DMA_REQUEST_I2C1_TX;
hdma_i2c1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_i2c1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_i2c1_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_i2c1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_i2c1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_i2c1_tx.Init.Mode = DMA_NORMAL;
hdma_i2c1_tx.Init.Priority = DMA_PRIORITY_HIGH;
if (HAL_DMA_Init(&hdma_i2c1_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(hi2c,hdmatx,hdma_i2c1_tx);
/* I2C1_RX Init */
hdma_i2c1_rx.Instance = DMA1_Channel4;
hdma_i2c1_rx.Init.Request = DMA_REQUEST_I2C1_RX;
hdma_i2c1_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_i2c1_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_i2c1_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_i2c1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_i2c1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_i2c1_rx.Init.Mode = DMA_NORMAL;
hdma_i2c1_rx.Init.Priority = DMA_PRIORITY_HIGH;
if (HAL_DMA_Init(&hdma_i2c1_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(hi2c,hdmarx,hdma_i2c1_rx);
/* I2C1 interrupt Init */
HAL_NVIC_SetPriority(I2C1_EV_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(I2C1_EV_IRQn);
HAL_NVIC_SetPriority(I2C1_ER_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(I2C1_ER_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 DMA DeInit */
HAL_DMA_DeInit(hi2c->hdmatx);
HAL_DMA_DeInit(hi2c->hdmarx);
/* I2C1 interrupt DeInit */
HAL_NVIC_DisableIRQ(I2C1_EV_IRQn);
HAL_NVIC_DisableIRQ(I2C1_ER_IRQn);
/* USER CODE BEGIN I2C1_MspDeInit 1 */
/* USER CODE END I2C1_MspDeInit 1 */
}
}
/**
* @brief IPCC MSP Initialization
* This function configures the hardware resources used in this example
* @param hipcc: IPCC handle pointer
* @retval None
*/
void HAL_IPCC_MspInit(IPCC_HandleTypeDef* hipcc)
{
if(hipcc->Instance==IPCC)
{
/* USER CODE BEGIN IPCC_MspInit 0 */
/* USER CODE END IPCC_MspInit 0 */
/* Peripheral clock enable */
__HAL_RCC_IPCC_CLK_ENABLE();
/* IPCC interrupt Init */
HAL_NVIC_SetPriority(IPCC_C1_RX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(IPCC_C1_RX_IRQn);
HAL_NVIC_SetPriority(IPCC_C1_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(IPCC_C1_TX_IRQn);
/* USER CODE BEGIN IPCC_MspInit 1 */
/* USER CODE END IPCC_MspInit 1 */
}
}
/**
* @brief IPCC MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param hipcc: IPCC handle pointer
* @retval None
*/
void HAL_IPCC_MspDeInit(IPCC_HandleTypeDef* hipcc)
{
if(hipcc->Instance==IPCC)
{
/* USER CODE BEGIN IPCC_MspDeInit 0 */
/* USER CODE END IPCC_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_IPCC_CLK_DISABLE();
/* IPCC interrupt DeInit */
HAL_NVIC_DisableIRQ(IPCC_C1_RX_IRQn);
HAL_NVIC_DisableIRQ(IPCC_C1_TX_IRQn);
/* USER CODE BEGIN IPCC_MspDeInit 1 */
/* USER CODE END IPCC_MspDeInit 1 */
}
}
/**
* @brief RTC MSP Initialization
* This function configures the hardware resources used in this example
* @param hrtc: RTC handle pointer
* @retval None
*/
void HAL_RTC_MspInit(RTC_HandleTypeDef* hrtc)
{
RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
if(hrtc->Instance==RTC)
{
/* USER CODE BEGIN RTC_MspInit 0 */
/* USER CODE END RTC_MspInit 0 */
/** Enable access to the backup domain
*/
HAL_PWR_EnableBkUpAccess();
/** Initializes the peripherals clock
*/
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;
PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
{
Error_Handler();
}
/* Peripheral clock enable */
__HAL_RCC_RTC_ENABLE();
__HAL_RCC_RTCAPB_CLK_ENABLE();
/* RTC interrupt Init */
HAL_NVIC_SetPriority(RTC_WKUP_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(RTC_WKUP_IRQn);
/* USER CODE BEGIN RTC_MspInit 1 */
/* USER CODE END RTC_MspInit 1 */
}
}
/**
* @brief RTC MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param hrtc: RTC handle pointer
* @retval None
*/
void HAL_RTC_MspDeInit(RTC_HandleTypeDef* hrtc)
{
if(hrtc->Instance==RTC)
{
/* USER CODE BEGIN RTC_MspDeInit 0 */
/* USER CODE END RTC_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_RTC_DISABLE();
__HAL_RCC_RTCAPB_CLK_DISABLE();
/* RTC interrupt DeInit */
HAL_NVIC_DisableIRQ(RTC_WKUP_IRQn);
/* USER CODE BEGIN RTC_MspDeInit 1 */
/* USER CODE END RTC_MspDeInit 1 */
}
}
/**
* @brief SPI MSP Initialization
* This function configures the hardware resources used in this example
* @param hspi: SPI handle pointer
* @retval None
*/
void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(hspi->Instance==SPI1)
{
/* USER CODE BEGIN SPI1_MspInit 0 */
/* USER CODE END SPI1_MspInit 0 */
/* Peripheral clock enable */
__HAL_RCC_SPI1_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/**SPI1 GPIO Configuration
PA5 ------> SPI1_SCK
PA6 ------> SPI1_MISO
PA7 ------> SPI1_MOSI
*/
GPIO_InitStruct.Pin = SPI1_IMU_SCK_Pin|SPI1_IMU_MISO_Pin|SPI1_IMU_MOSI_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* SPI1 DMA Init */
/* SPI1_TX Init */
hdma_spi1_tx.Instance = DMA1_Channel1;
hdma_spi1_tx.Init.Request = DMA_REQUEST_SPI1_TX;
hdma_spi1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_spi1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_spi1_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_spi1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_spi1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_spi1_tx.Init.Mode = DMA_NORMAL;
hdma_spi1_tx.Init.Priority = DMA_PRIORITY_HIGH;
if (HAL_DMA_Init(&hdma_spi1_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(hspi,hdmatx,hdma_spi1_tx);
/* SPI1_RX Init */
hdma_spi1_rx.Instance = DMA1_Channel2;
hdma_spi1_rx.Init.Request = DMA_REQUEST_SPI1_RX;
hdma_spi1_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_spi1_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_spi1_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_spi1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_spi1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_spi1_rx.Init.Mode = DMA_NORMAL;
hdma_spi1_rx.Init.Priority = DMA_PRIORITY_HIGH;
if (HAL_DMA_Init(&hdma_spi1_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(hspi,hdmarx,hdma_spi1_rx);
/* USER CODE BEGIN SPI1_MspInit 1 */
/* USER CODE END SPI1_MspInit 1 */
}
else if(hspi->Instance==SPI2)
{
/* USER CODE BEGIN SPI2_MspInit 0 */
/* USER CODE END SPI2_MspInit 0 */
/* Peripheral clock enable */
__HAL_RCC_SPI2_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
/**SPI2 GPIO Configuration
PC1 ------> SPI2_MOSI
PC2 ------> SPI2_MISO
PA9 ------> SPI2_SCK
*/
GPIO_InitStruct.Pin = SPI2_SD_MOSI_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF3_SPI2;
HAL_GPIO_Init(SPI2_SD_MOSI_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = SPI2_SD_MISO_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF5_SPI2;
HAL_GPIO_Init(SPI2_SD_MISO_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = SPI2_SD_SCK_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF5_SPI2;
HAL_GPIO_Init(SPI2_SD_SCK_GPIO_Port, &GPIO_InitStruct);
/* USER CODE BEGIN SPI2_MspInit 1 */
/* USER CODE END SPI2_MspInit 1 */
}
}
/**
* @brief SPI MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param hspi: SPI handle pointer
* @retval None
*/
void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi)
{
if(hspi->Instance==SPI1)
{
/* USER CODE BEGIN SPI1_MspDeInit 0 */
/* USER CODE END SPI1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_SPI1_CLK_DISABLE();
/**SPI1 GPIO Configuration
PA5 ------> SPI1_SCK
PA6 ------> SPI1_MISO
PA7 ------> SPI1_MOSI
*/
HAL_GPIO_DeInit(GPIOA, SPI1_IMU_SCK_Pin|SPI1_IMU_MISO_Pin|SPI1_IMU_MOSI_Pin);
/* SPI1 DMA DeInit */
HAL_DMA_DeInit(hspi->hdmatx);
HAL_DMA_DeInit(hspi->hdmarx);
/* USER CODE BEGIN SPI1_MspDeInit 1 */
/* USER CODE END SPI1_MspDeInit 1 */
}
else if(hspi->Instance==SPI2)
{
/* USER CODE BEGIN SPI2_MspDeInit 0 */
/* USER CODE END SPI2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_SPI2_CLK_DISABLE();
/**SPI2 GPIO Configuration
PC1 ------> SPI2_MOSI
PC2 ------> SPI2_MISO
PA9 ------> SPI2_SCK
*/
HAL_GPIO_DeInit(GPIOC, SPI2_SD_MOSI_Pin|SPI2_SD_MISO_Pin);
HAL_GPIO_DeInit(SPI2_SD_SCK_GPIO_Port, SPI2_SD_SCK_Pin);
/* USER CODE BEGIN SPI2_MspDeInit 1 */
/* USER CODE END SPI2_MspDeInit 1 */
}
}
/**
* @brief TIM_PWM MSP Initialization
* This function configures the hardware resources used in this example
* @param htim_pwm: TIM_PWM handle pointer
* @retval None
*/
void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* htim_pwm)
{
if(htim_pwm->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==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
*/
GPIO_InitStruct.Pin = GPIO_CONNECT_LED_Pin|GPIO_STATUS_LED_Pin|GPIO_DEBUG_LED_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM2;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USER CODE BEGIN TIM2_MspPostInit 1 */
/* USER CODE END TIM2_MspPostInit 1 */
}
}
/**
* @brief TIM_PWM MSP De-Initialization
* This function freeze the hardware resources used in this example
* @param htim_pwm: TIM_PWM handle pointer
* @retval None
*/
void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* htim_pwm)
{
if(htim_pwm->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 */
Core/Src/stm32wbxx_hal_timebase_tim.c
+136
View File
@@ -0,0 +1,136 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32wbxx_hal_timebase_tim.c
* @brief HAL time base based on the hardware TIM.
******************************************************************************
* @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 "stm32wbxx_hal.h"
#include "stm32wbxx_hal_tim.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim16;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/**
* @brief This function configures the TIM16 as a time base source.
* The time source is configured to have 1ms time base with a dedicated
* Tick interrupt priority.
* @note This function is called automatically at the beginning of program after
* reset by HAL_Init() or at any time when clock is configured, by HAL_RCC_ClockConfig().
* @param TickPriority: Tick interrupt priority.
* @retval HAL status
*/
HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
RCC_ClkInitTypeDef clkconfig;
uint32_t uwTimclock = 0;
uint32_t uwPrescalerValue = 0;
uint32_t pFLatency;
HAL_StatusTypeDef status = HAL_OK;
/*Configure the TIM16 IRQ priority */
HAL_NVIC_SetPriority(TIM1_UP_TIM16_IRQn, TickPriority ,0);
/* Enable the TIM16 global Interrupt */
HAL_NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);
/* Enable TIM16 clock */
__HAL_RCC_TIM16_CLK_ENABLE();
/* Get clock configuration */
HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);
/* Compute TIM16 clock */
uwTimclock = HAL_RCC_GetPCLK2Freq();
/* Compute the prescaler value to have TIM16 counter clock equal to 1MHz */
uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000U) - 1U);
/* Initialize TIM16 */
htim16.Instance = TIM16;
/* Initialize TIMx peripheral as follow:
* Period = [(TIM16CLK/1000) - 1]. to have a (1/1000) s time base.
* Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
* ClockDivision = 0
* Counter direction = Up
*/
htim16.Init.Period = (1000000U / 1000U) - 1U;
htim16.Init.Prescaler = uwPrescalerValue;
htim16.Init.ClockDivision = 0;
htim16.Init.CounterMode = TIM_COUNTERMODE_UP;
status = HAL_TIM_Base_Init(&htim16);
if (status == HAL_OK)
{
#if (USE_HAL_TIM_REGISTER_CALLBACKS == 1U)
/* Register callback */
HAL_TIM_RegisterCallback(&htim16, HAL_TIM_PERIOD_ELAPSED_CB_ID, TimeBase_TIM_PeriodElapsedCallback);
#endif /* USE_HAL_TIM_REGISTER_CALLBACKS */
/* Start the TIM time Base generation in interrupt mode */
status = HAL_TIM_Base_Start_IT(&htim16);
if (status == HAL_OK)
{
/* Enable the TIM16 global Interrupt */
HAL_NVIC_EnableIRQ(TIM1_UP_TIM16_IRQn);
/* Configure the SysTick IRQ priority */
if (TickPriority < (1UL << __NVIC_PRIO_BITS))
{
/* Configure the TIM IRQ priority */
HAL_NVIC_SetPriority(TIM1_UP_TIM16_IRQn, TickPriority, 0U);
uwTickPrio = TickPriority;
}
else
{
status = HAL_ERROR;
}
}
}
/* Return function status */
return status;
}
/**
* @brief Suspend Tick increment.
* @note Disable the tick increment by disabling TIM16 update interrupt.
* @param None
* @retval None
*/
void HAL_SuspendTick(void)
{
/* Disable TIM16 update Interrupt */
__HAL_TIM_DISABLE_IT(&htim16, TIM_IT_UPDATE);
}
/**
* @brief Resume Tick increment.
* @note Enable the tick increment by Enabling TIM16 update interrupt.
* @param None
* @retval None
*/
void HAL_ResumeTick(void)
{
/* Enable TIM16 Update interrupt */
__HAL_TIM_ENABLE_IT(&htim16, TIM_IT_UPDATE);
}
Core/Src/stm32wbxx_it.c
+340
View File
@@ -0,0 +1,340 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32wbxx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @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"
#include "stm32wbxx_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 PCD_HandleTypeDef hpcd_USB_FS;
extern DMA_HandleTypeDef hdma_i2c1_tx;
extern DMA_HandleTypeDef hdma_i2c1_rx;
extern I2C_HandleTypeDef hi2c1;
extern IPCC_HandleTypeDef hipcc;
extern RTC_HandleTypeDef hrtc;
extern DMA_HandleTypeDef hdma_spi1_tx;
extern DMA_HandleTypeDef hdma_spi1_rx;
extern TIM_HandleTypeDef htim16;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex 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 Memory management fault.
*/
void MemManage_Handler(void)
{
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
/* USER CODE END W1_MemoryManagement_IRQn 0 */
}
}
/**
* @brief This function handles Prefetch fault, memory access fault.
*/
void BusFault_Handler(void)
{
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_BusFault_IRQn 0 */
/* USER CODE END W1_BusFault_IRQn 0 */
}
}
/**
* @brief This function handles Undefined instruction or illegal state.
*/
void UsageFault_Handler(void)
{
/* USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_UsageFault_IRQn 0 */
/* USER CODE END W1_UsageFault_IRQn 0 */
}
}
/**
* @brief This function handles Debug monitor.
*/
void DebugMon_Handler(void)
{
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
/* USER CODE END DebugMonitor_IRQn 0 */
/* USER CODE BEGIN DebugMonitor_IRQn 1 */
/* USER CODE END DebugMonitor_IRQn 1 */
}
/******************************************************************************/
/* STM32WBxx 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_stm32wbxx.s). */
/******************************************************************************/
/**
* @brief This function handles RTC wake-up interrupt through EXTI line 19.
*/
void RTC_WKUP_IRQHandler(void)
{
/* USER CODE BEGIN RTC_WKUP_IRQn 0 */
/* USER CODE END RTC_WKUP_IRQn 0 */
HAL_RTCEx_WakeUpTimerIRQHandler(&hrtc);
/* USER CODE BEGIN RTC_WKUP_IRQn 1 */
/* USER CODE END RTC_WKUP_IRQn 1 */
}
/**
* @brief This function handles DMA1 channel1 global interrupt.
*/
void DMA1_Channel1_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Channel1_IRQn 0 */
/* USER CODE END DMA1_Channel1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_spi1_tx);
/* USER CODE BEGIN DMA1_Channel1_IRQn 1 */
/* USER CODE END DMA1_Channel1_IRQn 1 */
}
/**
* @brief This function handles DMA1 channel2 global interrupt.
*/
void DMA1_Channel2_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Channel2_IRQn 0 */
/* USER CODE END DMA1_Channel2_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_spi1_rx);
/* USER CODE BEGIN DMA1_Channel2_IRQn 1 */
/* USER CODE END DMA1_Channel2_IRQn 1 */
}
/**
* @brief This function handles DMA1 channel3 global interrupt.
*/
void DMA1_Channel3_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Channel3_IRQn 0 */
/* USER CODE END DMA1_Channel3_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_i2c1_tx);
/* USER CODE BEGIN DMA1_Channel3_IRQn 1 */
/* USER CODE END DMA1_Channel3_IRQn 1 */
}
/**
* @brief This function handles DMA1 channel4 global interrupt.
*/
void DMA1_Channel4_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Channel4_IRQn 0 */
/* USER CODE END DMA1_Channel4_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_i2c1_rx);
/* USER CODE BEGIN DMA1_Channel4_IRQn 1 */
/* USER CODE END DMA1_Channel4_IRQn 1 */
}
/**
* @brief This function handles USB low priority interrupt, USB wake-up interrupt through EXTI line 28.
*/
void USB_LP_IRQHandler(void)
{
/* USER CODE BEGIN USB_LP_IRQn 0 */
/* USER CODE END USB_LP_IRQn 0 */
HAL_PCD_IRQHandler(&hpcd_USB_FS);
/* USER CODE BEGIN USB_LP_IRQn 1 */
/* USER CODE END USB_LP_IRQn 1 */
}
/**
* @brief This function handles TIM1 update interrupt and TIM16 global interrupt.
*/
void TIM1_UP_TIM16_IRQHandler(void)
{
/* USER CODE BEGIN TIM1_UP_TIM16_IRQn 0 */
/* USER CODE END TIM1_UP_TIM16_IRQn 0 */
HAL_TIM_IRQHandler(&htim16);
/* USER CODE BEGIN TIM1_UP_TIM16_IRQn 1 */
/* USER CODE END TIM1_UP_TIM16_IRQn 1 */
}
/**
* @brief This function handles I2C1 event interrupt.
*/
void I2C1_EV_IRQHandler(void)
{
/* USER CODE BEGIN I2C1_EV_IRQn 0 */
/* USER CODE END I2C1_EV_IRQn 0 */
HAL_I2C_EV_IRQHandler(&hi2c1);
/* USER CODE BEGIN I2C1_EV_IRQn 1 */
/* USER CODE END I2C1_EV_IRQn 1 */
}
/**
* @brief This function handles I2C1 error interrupt.
*/
void I2C1_ER_IRQHandler(void)
{
/* USER CODE BEGIN I2C1_ER_IRQn 0 */
/* USER CODE END I2C1_ER_IRQn 0 */
HAL_I2C_ER_IRQHandler(&hi2c1);
/* USER CODE BEGIN I2C1_ER_IRQn 1 */
/* USER CODE END I2C1_ER_IRQn 1 */
}
/**
* @brief This function handles IPCC RX occupied interrupt.
*/
void IPCC_C1_RX_IRQHandler(void)
{
/* USER CODE BEGIN IPCC_C1_RX_IRQn 0 */
/* USER CODE END IPCC_C1_RX_IRQn 0 */
HAL_IPCC_RX_IRQHandler(&hipcc);
/* USER CODE BEGIN IPCC_C1_RX_IRQn 1 */
/* USER CODE END IPCC_C1_RX_IRQn 1 */
}
/**
* @brief This function handles IPCC TX free interrupt.
*/
void IPCC_C1_TX_IRQHandler(void)
{
/* USER CODE BEGIN IPCC_C1_TX_IRQn 0 */
/* USER CODE END IPCC_C1_TX_IRQn 0 */
HAL_IPCC_TX_IRQHandler(&hipcc);
/* USER CODE BEGIN IPCC_C1_TX_IRQn 1 */
/* USER CODE END IPCC_C1_TX_IRQn 1 */
}
/**
* @brief This function handles HSEM global interrupt.
*/
void HSEM_IRQHandler(void)
{
/* USER CODE BEGIN HSEM_IRQn 0 */
/* USER CODE END HSEM_IRQn 0 */
HAL_HSEM_IRQHandler();
/* USER CODE BEGIN HSEM_IRQn 1 */
/* USER CODE END HSEM_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
Core/Src/syscalls.c
+176
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-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.
*
******************************************************************************
*/
/* 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;
}
Core/Src/sysmem.c
+86
View File
@@ -0,0 +1,86 @@
/**
******************************************************************************
* @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) 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.
*
******************************************************************************
*/
/* Includes */
#include <errno.h>
#include <stdint.h>
#include <stddef.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;
}
#if defined(__PICOLIBC__)
// Picolibc expects syscalls without the leading underscore.
// This creates a strong alias so that
// calls to `sbrk()` are resolved to our `_sbrk()` implementation.
__strong_reference(_sbrk, sbrk);
#endif
Core/Src/system_stm32wbxx.c
+378
View File
@@ -0,0 +1,378 @@
/**
******************************************************************************
* @file system_stm32wbxx.c
* @author MCD Application Team
* @brief CMSIS Cortex 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_stm32wbxx.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 MSI (4 MHz) is used as system clock source.
* Then SystemInit() function is called, in "startup_stm32wbxx.s" file, to
* configure the system clock before to branch to main program.
*
* This file configures the system clock as follows:
*=============================================================================
*-----------------------------------------------------------------------------
* System Clock source | MSI
*-----------------------------------------------------------------------------
* SYSCLK(Hz) | 4000000
*-----------------------------------------------------------------------------
* HCLK(Hz) | 4000000
*-----------------------------------------------------------------------------
* AHB Prescaler | 1
*-----------------------------------------------------------------------------
* APB1 Prescaler | 1
*-----------------------------------------------------------------------------
* APB2 Prescaler | 1
*-----------------------------------------------------------------------------
* PLL_M | 1
*-----------------------------------------------------------------------------
* PLL_N | 8
*-----------------------------------------------------------------------------
* PLL_P | 7
*-----------------------------------------------------------------------------
* PLL_Q | 2
*-----------------------------------------------------------------------------
* PLL_R | 2
*-----------------------------------------------------------------------------
* PLLSAI1_P | NA
*-----------------------------------------------------------------------------
* PLLSAI1_Q | NA
*-----------------------------------------------------------------------------
* PLLSAI1_R | NA
*-----------------------------------------------------------------------------
* Require 48MHz for USB OTG FS, | Disabled
* SDIO and RNG clock |
*-----------------------------------------------------------------------------
*=============================================================================
******************************************************************************
* @attention
*
* Copyright (c) 2019-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 stm32WBxx_system
* @{
*/
/** @addtogroup stm32WBxx_System_Private_Includes
* @{
*/
#include "stm32wbxx.h"
#if !defined (HSE_VALUE)
#define HSE_VALUE (32000000UL) /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (MSI_VALUE)
#define MSI_VALUE (4000000UL) /*!< Value of the Internal oscillator in Hz*/
#endif /* MSI_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)
#if defined(STM32WB5Mxx)
#define LSE_VALUE 32774U /*!< Value of the LSE oscillator in Hz */
#else
#define LSE_VALUE 32768U /*!< Value of the LSE oscillator in Hz */
#endif /* STM32WB5Mxx */
#endif /* LSE_VALUE */
/**
* @}
*/
/** @addtogroup STM32WBxx_System_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @addtogroup STM32WBxx_System_Private_Defines
* @{
*/
/* Note: Following vector table addresses must be defined in line with linker
configuration. */
/*!< Uncomment the following line if you need to relocate CPU1 CM4 and/or CPU2
CM0+ vector table anywhere in Sram or Flash. Else vector table will be kept
at address 0x00 which correspond to automatic remap of boot address selected */
/* #define USER_VECT_TAB_ADDRESS */
#if defined(USER_VECT_TAB_ADDRESS)
/*!< Uncomment this line for user vector table remap in Sram else user remap
will be done in Flash. */
/* #define VECT_TAB_SRAM */
#if defined(VECT_TAB_SRAM)
#define VECT_TAB_BASE_ADDRESS SRAM1_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#if !defined(VECT_TAB_OFFSET)
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table offset field.
This value must be a multiple of 0x200. */
#endif /* VECT_TAB_OFFSET */
#else
#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#if !defined(VECT_TAB_OFFSET)
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table offset field.
This value must be a multiple of 0x200. */
#endif /* VECT_TAB_OFFSET */
#endif /* VECT_TAB_SRAM */
#endif /* USER_VECT_TAB_ADDRESS */
/**
* @}
*/
/** @addtogroup STM32WBxx_System_Private_Macros
* @{
*/
/**
* @}
*/
/** @addtogroup STM32WBxx_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 = 4000000UL ; /*CPU1: M4 on MSI clock after startup (4MHz)*/
const uint32_t AHBPrescTable[16UL] = {1UL, 3UL, 5UL, 1UL, 1UL, 6UL, 10UL, 32UL, 2UL, 4UL, 8UL, 16UL, 64UL, 128UL, 256UL, 512UL};
const uint32_t APBPrescTable[8UL] = {0UL, 0UL, 0UL, 0UL, 1UL, 2UL, 3UL, 4UL};
const uint32_t MSIRangeTable[16UL] = {100000UL, 200000UL, 400000UL, 800000UL, 1000000UL, 2000000UL, \
4000000UL, 8000000UL, 16000000UL, 24000000UL, 32000000UL, 48000000UL, 0UL, 0UL, 0UL, 0UL
}; /* 0UL values are incorrect cases */
#if defined(STM32WB55xx) || defined(STM32WB5Mxx) || defined(STM32WB35xx) || defined (STM32WB15xx) || defined (STM32WB1Mxx)
const uint32_t SmpsPrescalerTable[4UL][6UL] = {{1UL, 3UL, 2UL, 2UL, 1UL, 2UL}, \
{2UL, 6UL, 4UL, 3UL, 2UL, 4UL}, \
{4UL, 12UL, 8UL, 6UL, 4UL, 8UL}, \
{4UL, 12UL, 8UL, 6UL, 4UL, 8UL}
};
#endif /* STM32WB55xx || STM32WB5Mxx || STM32WB35xx || STM32WB15xx || STM32WB1Mxx */
/**
* @}
*/
/** @addtogroup STM32WBxx_System_Private_FunctionPrototypes
* @{
*/
/**
* @}
*/
/** @addtogroup STM32WBxx_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system.
* @param None
* @retval None
*/
void SystemInit(void)
{
#if defined(USER_VECT_TAB_ADDRESS)
/* Configure the Vector Table location add offset address ------------------*/
SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET;
#endif /* USER_VECT_TAB_ADDRESS */
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << (10UL * 2UL)) | (3UL << (11UL * 2UL))); /* set CP10 and CP11 Full Access */
#endif /* FPU */
/* Reset the RCC clock configuration to the default reset state ------------*/
/* Set MSION bit */
RCC->CR |= RCC_CR_MSION;
/* Reset CFGR register */
RCC->CFGR = 0x00070000U;
/* Reset PLLSAI1ON, PLLON, HSECSSON, HSEON, HSION, and MSIPLLON bits */
RCC->CR &= (uint32_t)0xFAF6FEFBU;
/*!< Reset LSI1 and LSI2 bits */
RCC->CSR &= (uint32_t)0xFFFFFFFAU;
/*!< Reset HSI48ON bit */
RCC->CRRCR &= (uint32_t)0xFFFFFFFEU;
/* Reset PLLCFGR register */
RCC->PLLCFGR = 0x22041000U;
#if defined(STM32WB55xx) || defined(STM32WB5Mxx)
/* Reset PLLSAI1CFGR register */
RCC->PLLSAI1CFGR = 0x22041000U;
#endif /* STM32WB55xx || STM32WB5Mxx */
/* Reset HSEBYP bit */
RCC->CR &= 0xFFFBFFFFU;
/* Disable all interrupts */
RCC->CIER = 0x00000000;
}
/**
* @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 MSI, SystemCoreClock will contain the MSI_VALUE(*)
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***)
* or HSI_VALUE(*) or MSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) MSI_VALUE is a constant defined in stm32wbxx_hal.h file (default value
* 4 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSI_VALUE is a constant defined in stm32wbxx_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 stm32wbxx_hal_conf.h file (default value
* 32 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, msirange, pllvco, pllr, pllsource, pllm;
/* Get MSI Range frequency--------------------------------------------------*/
/*MSI frequency range in Hz*/
msirange = MSIRangeTable[(RCC->CR & RCC_CR_MSIRANGE) >> RCC_CR_MSIRANGE_Pos];
/* Get SYSCLK source -------------------------------------------------------*/
switch (RCC->CFGR & RCC_CFGR_SWS)
{
case 0x00: /* MSI used as system clock source */
SystemCoreClock = msirange;
break;
case 0x04: /* HSI used as system clock source */
/* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x08: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x0C: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE or MSI_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 == 0x02UL) /* HSI used as PLL clock source */
{
pllvco = (HSI_VALUE / pllm);
}
else if (pllsource == 0x03UL) /* HSE used as PLL clock source */
{
pllvco = (HSE_VALUE / pllm);
}
else /* MSI used as PLL clock source */
{
pllvco = (msirange / 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;
default:
SystemCoreClock = msirange;
break;
}
/* Compute HCLK clock frequency --------------------------------------------*/
/* Get HCLK1 prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> RCC_CFGR_HPRE_Pos)];
/* HCLK clock frequency */
SystemCoreClock = SystemCoreClock / tmp;
}
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/
Core/Src/trinity_helperlib.c
+4
View File
@@ -0,0 +1,4 @@
#include "trinity_helperlib.h"
Core/Src/trinity_helperlib.h
+13
View File
@@ -0,0 +1,13 @@
/*
* trinity_helperlib.h
*
* Created on: Aug 17, 2026
* Author: pima
*/
#ifndef SRC_TRINITY_HELPERLIB_H_
#define SRC_TRINITY_HELPERLIB_H_
#endif /* SRC_TRINITY_HELPERLIB_H_ */