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.

2026-09-13 19:51:03 +02:00
commit 7ee381de58
1630 changed files with 632404 additions and 0 deletions
@@ -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 */