/* 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 */