pima/Trinity-DevPCB-V1.0
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
master
Trinity-DevPCB-V1.0/Core/Src/main.c
T

969 lines
29 KiB
C
Raw Permalink Blame History

/* 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 */
Reference in New Issue View Git Blame Copy Permalink