text: /**
   ******************************************************************************
   * File Name          : app_mems.c
   * Description        : This file provides code for the configuration
   *                      of the STMicroelectronics.X-CUBE-MEMS1.9.2.0 instances.
   ******************************************************************************
   * @attention
   *
   * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
   * All rights reserved.</center></h2>
   *
   * This software component is licensed by ST under Software License Agreement SLA0077,
   * the "License". You may not use this component except in compliance with the
   * License. You may obtain a copy of the License at:
   *                        www.st.com/sla0077
   *
   ******************************************************************************
   */
  
  #ifdef __cplusplus
  extern "C" {
  #endif
  
  /* Includes ------------------------------------------------------------------*/
  #include "app_mems.h"
  #include "main.h"
  #include <stdio.h>
  
  #include "stm32l0xx_hal.h"
  #include "stm32l0xx_nucleo.h"
  #include "com.h"
  #include "demo_serial.h"
  #include "bsp_ip_conf.h"
  #include "fw_version.h"
  #include "motion_fx_manager.h"
  
  /* Private typedef -----------------------------------------------------------*/
  /* Private define ------------------------------------------------------------*/
  #define DWT_LAR_KEY  0xC5ACCE55 /* DWT register unlock key */
  #define ALGO_FREQ  100U /* Algorithm frequency 100Hz */
  #define ACC_ODR  ((float)ALGO_FREQ)
  #define ACC_FS  4 /* FS = <-4g, 4g> */
  #define ALGO_PERIOD  (1000U / ALGO_FREQ) /* Algorithm period [ms] */
  #define MOTION_FX_ENGINE_DELTATIME  0.01f
  #define FROM_MG_TO_G  0.001f
  #define FROM_G_TO_MG  1000.0f
  #define FROM_MDPS_TO_DPS  0.001f
  #define FROM_DPS_TO_MDPS  1000.0f
  #define FROM_MGAUSS_TO_UT50  (0.1f/50.0f)
  #define FROM_UT50_TO_MGAUSS  500.0f
  
  /* Public variables ----------------------------------------------------------*/
  volatile uint8_t DataLoggerActive = 0;
  volatile uint32_t SensorsEnabled = 0;
  char LibVersion[35];
  int LibVersionLen;
  volatile uint8_t SensorReadRequest = 0;
  uint8_t UseOfflineData = 0;
  offline_data_t OfflineData[OFFLINE_DATA_SIZE];
  int OfflineDataReadIndex = 0;
  int OfflineDataWriteIndex = 0;
  int OfflineDataCount = 0;
  uint32_t AlgoFreq = ALGO_FREQ;
  uint8_t Enabled6X = 0;
  static int32_t PushButtonState = GPIO_PIN_RESET;
  
  /* Extern variables ----------------------------------------------------------*/
  /* Private macro -------------------------------------------------------------*/
  /* Private variables ---------------------------------------------------------*/
  static MOTION_SENSOR_Axes_t AccValue;
  static MOTION_SENSOR_Axes_t GyrValue;
  static MOTION_SENSOR_Axes_t MagValue;
  static float PressValue;
  static float TempValue;
  static float HumValue;
  static volatile uint8_t MagCalRequest = 0;
  static MOTION_SENSOR_Axes_t MagOffset;
  static uint8_t MagCalStatus = 0;
  
  /* Private function prototypes -----------------------------------------------*/
  static void MX_DataLogFusion_Init(void);
  static void MX_DataLogFusion_Process(void);
  static void FX_Data_Handler(TMsg *Msg);
  static void Init_Sensors(void);
  static void RTC_Handler(TMsg *Msg);
  static void Accelero_Sensor_Handler(TMsg *Msg);
  static void Gyro_Sensor_Handler(TMsg *Msg);
  static void Magneto_Sensor_Handler(TMsg *Msg);
  static void Pressure_Sensor_Handler(TMsg *Msg);
  static void Temperature_Sensor_Handler(TMsg *Msg);
  static void Humidity_Sensor_Handler(TMsg *Msg);
  static void TIM_Config(uint32_t Freq);
  
  void MX_MEMS_Init(void)
  {
    /* USER CODE BEGIN SV */
  
    /* USER CODE END SV */
  
    /* USER CODE BEGIN MEMS_Init_PreTreatment */
  
    /* USER CODE END MEMS_Init_PreTreatment */
  
    /* Initialize the peripherals and the MEMS components */
  
    MX_DataLogFusion_Init();
  
    /* USER CODE BEGIN MEMS_Init_PostTreatment */
  
    /* USER CODE END MEMS_Init_PostTreatment */
  }
  
  /*
   * LM background task
   */
  void MX_MEMS_Process(void)
  {
    /* USER CODE BEGIN MEMS_Process_PreTreatment */
  
    /* USER CODE END MEMS_Process_PreTreatment */
  
    MX_DataLogFusion_Process();
  
    /* USER CODE BEGIN MEMS_Process_PostTreatment */
  
    /* USER CODE END MEMS_Process_PostTreatment */
  }
  
  /* Exported functions --------------------------------------------------------*/
  /**
   * @brief  Period elapsed callback
   * @param  htim pointer to a TIM_HandleTypeDef structure that contains
   *              the configuration information for TIM module.
   * @retval None
   */
  void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  {
    if (htim->Instance == BSP_IP_TIM_Handle.Instance)
    {
      SensorReadRequest = 1;
    }
  }
  
  /* Private functions ---------------------------------------------------------*/
  /**
    * @brief  Initialize the application
    * @retval None
    */
  static void MX_DataLogFusion_Init(void)
  {
    float ans_float;
  
    /* Initialize button */
    BSP_PB_Init(BUTTON_KEY, BUTTON_MODE_EXTI);
  
    /* Check what is the Push Button State when the button is not pressed. It can change across families */
    PushButtonState = (BSP_PB_GetState(BUTTON_KEY)) ?  0 : 1;
  
    /* Initialize LED */
    BSP_LED_Init(LED2);
  
    /* Initialize Virtual COM Port */
    BSP_COM_Init(COM1);
  
    /* Initialize Timer */
    BSP_IP_TIM_Init();
  
    /* Configure Timer to run with desired algorithm frequency */
    TIM_Config(ALGO_FREQ);
  
    /* Initialize (disabled) sensors */
    Init_Sensors();
  
    /* Sensor Fusion API initialization function */
    MotionFX_manager_init();
  
    /* OPTIONAL */
    /* Get library version */
    MotionFX_manager_get_version(LibVersion, &LibVersionLen);
  
    /* Enable magnetometer calibration */
    MotionFX_manager_MagCal_start(ALGO_PERIOD);
  
    /* Test if calibration data are available */
    MFX_CM0P_MagCal_output_t mag_cal_test;
    MotionFX_CM0P_MagCal_getParams(&mag_cal_test);
  
    /* If calibration data are available load HI coefficients */
    if (mag_cal_test.CalQuality == MFX_CM0P_CALQSTATUSBEST)
    {
      ans_float = (mag_cal_test.HI_Bias[0] * FROM_UT50_TO_MGAUSS);
      MagOffset.x = (int32_t)ans_float;
      ans_float = (mag_cal_test.HI_Bias[1] * FROM_UT50_TO_MGAUSS);
      MagOffset.y = (int32_t)ans_float;
      ans_float = (mag_cal_test.HI_Bias[2] * FROM_UT50_TO_MGAUSS);
      MagOffset.z = (int32_t)ans_float;
  
      MagCalStatus = 1;
    }
  
    BSP_LED_On(LED2);
    HAL_Delay(500);
    BSP_LED_Off(LED2);
  
    /* Start receiving messages via DMA */
    UART_StartReceiveMsg();
  }
  
  /**
    * @brief  Process of the application
    * @retval None
    */
  static void MX_DataLogFusion_Process(void)
  {
    static TMsg msg_dat;
    static TMsg msg_cmd;
  
    if (UART_ReceivedMSG((TMsg *)&msg_cmd) == 1)
    {
      if (msg_cmd.Data[0] == DEV_ADDR)
      {
        (void)HandleMSG((TMsg *)&msg_cmd);
      }
    }
  
    if (MagCalRequest == 1U)
    {
      /* Debouncing */
      HAL_Delay(50);
  
      /* Wait until the button is released */
      while ((BSP_PB_GetState( BUTTON_KEY ) == PushButtonState));
  
      /* Debouncing */
      HAL_Delay(50);
  
      MagCalRequest = 0;
  
      /* Reset magnetometer calibration value*/
      MagCalStatus = 0;
      MagOffset.x = 0;
      MagOffset.y = 0;
      MagOffset.z = 0;
  
      /* Enable magnetometer calibration */
      MotionFX_manager_MagCal_start(ALGO_PERIOD);
    }
  
    if (SensorReadRequest == 1U)
    {
      SensorReadRequest = 0;
  
      /* Acquire data from enabled sensors and fill Msg stream */
      RTC_Handler(&msg_dat);
      Accelero_Sensor_Handler(&msg_dat);
      Gyro_Sensor_Handler(&msg_dat);
      Magneto_Sensor_Handler(&msg_dat);
      Humidity_Sensor_Handler(&msg_dat);
      Temperature_Sensor_Handler(&msg_dat);
      Pressure_Sensor_Handler(&msg_dat);
  
      /* Sensor Fusion specific part */
      FX_Data_Handler(&msg_dat);
  
      /* Send data stream */
      INIT_STREAMING_HEADER(&msg_dat);
      msg_dat.Len = STREAMING_MSG_LENGTH;
  
      if (UseOfflineData == 1U)
      {
        OfflineDataCount--;
        if (OfflineDataCount < 0)
        {
          OfflineDataCount = 0;
        }
  
        OfflineDataReadIndex++;
        if (OfflineDataReadIndex >= OFFLINE_DATA_SIZE)
        {
          OfflineDataReadIndex = 0;
        }
  
        if (OfflineDataCount > 0)
        {
          SensorReadRequest = 1;
        }
      }
      UART_SendMsg(&msg_dat);
    }
  }
  
  /**
   * @brief  Initialize all sensors
   * @param  None
   * @retval None
   */
  static void Init_Sensors(void)
  {
    BSP_SENSOR_ACC_Init();
    BSP_SENSOR_GYR_Init();
    BSP_SENSOR_MAG_Init();
    BSP_SENSOR_PRESS_Init();
    BSP_SENSOR_TEMP_Init();
    BSP_SENSOR_HUM_Init();
  
    BSP_SENSOR_ACC_SetOutputDataRate(ACC_ODR);
    BSP_SENSOR_ACC_SetFullScale(ACC_FS);
  }
  
  /**
   * @brief  Handles the time+date getting/sending
   * @param  Msg the time+date part of the stream
   * @retval None
   */
  static void RTC_Handler(TMsg *Msg)
  {
    uint8_t sub_sec = 0;
    RTC_DateTypeDef sdatestructureget;
    RTC_TimeTypeDef stimestructure;
    uint32_t ans_uint32;
    int32_t ans_int32;
    uint32_t RtcSynchPrediv = hrtc.Init.SynchPrediv;
  
    if (UseOfflineData == 1)
    {
      Msg->Data[3] = (uint8_t)OfflineData[OfflineDataReadIndex].hours;
      Msg->Data[4] = (uint8_t)OfflineData[OfflineDataReadIndex].minutes;
      Msg->Data[5] = (uint8_t)OfflineData[OfflineDataReadIndex].seconds;
      Msg->Data[6] = (uint8_t)OfflineData[OfflineDataReadIndex].subsec;
    }
    else
    {
      (void)HAL_RTC_GetTime(&hrtc, &stimestructure, FORMAT_BIN);
      (void)HAL_RTC_GetDate(&hrtc, &sdatestructureget, FORMAT_BIN);
  
      /* To be MISRA C-2012 compliant the original calculation:
         sub_sec = ((((((int)RtcSynchPrediv) - ((int)stimestructure.SubSeconds)) * 100) / (RtcSynchPrediv + 1)) & 0xFF);
         has been split to separate expressions */
      ans_int32 = (RtcSynchPrediv - (int32_t)stimestructure.SubSeconds) * 100;
      ans_int32 /= RtcSynchPrediv + 1;
      ans_uint32 = (uint32_t)ans_int32 & 0xFFU;
      sub_sec = (uint8_t)ans_uint32;
  
      Msg->Data[3] = (uint8_t)stimestructure.Hours;
      Msg->Data[4] = (uint8_t)stimestructure.Minutes;
      Msg->Data[5] = (uint8_t)stimestructure.Seconds;
      Msg->Data[6] = sub_sec;
    }
  }
  
  /**
   * @brief  Sensor Fusion data handler
   * @param  Msg the Sensor Fusion data part of the stream
   * @retval None
   */
  static void FX_Data_Handler(TMsg *Msg)
  {
    uint32_t elapsed_time_us = 0U;
    uint32_t prev_tick = 0U;
    MFX_CM0P_input_t data_in;
    MFX_CM0P_input_t *pdata_in = &data_in;
    MFX_CM0P_output_t data_out;
    MFX_CM0P_output_t *pdata_out = &data_out;
  
    if ((SensorsEnabled & ACCELEROMETER_SENSOR) == ACCELEROMETER_SENSOR)
    {
      if ((SensorsEnabled & GYROSCOPE_SENSOR) == GYROSCOPE_SENSOR)
      {
        if ((SensorsEnabled & MAGNETIC_SENSOR) == MAGNETIC_SENSOR)
        {
          /* Convert angular velocity from [mdps] to [dps] */
          data_in.gyro[0] = (float)GyrValue.x * FROM_MDPS_TO_DPS;
          data_in.gyro[1] = (float)GyrValue.y * FROM_MDPS_TO_DPS;
          data_in.gyro[2] = (float)GyrValue.z * FROM_MDPS_TO_DPS;
  
          /* Convert acceleration from [mg] to [g] */
          data_in.acc[0] = (float)AccValue.x * FROM_MG_TO_G;
          data_in.acc[1] = (float)AccValue.y * FROM_MG_TO_G;
          data_in.acc[2] = (float)AccValue.z * FROM_MG_TO_G;
  
          /* Convert magnetic field intensity from [mGauss] to [uT / 50] */
          data_in.mag[0] = (float)MagValue.x * FROM_MGAUSS_TO_UT50;
          data_in.mag[1] = (float)MagValue.y * FROM_MGAUSS_TO_UT50;
          data_in.mag[2] = (float)MagValue.z * FROM_MGAUSS_TO_UT50;
  
          /* Run Sensor Fusion algorithm */
          BSP_LED_On(LED2);
          prev_tick = HAL_GetTick();
          MotionFX_manager_run(pdata_in, pdata_out, MOTION_FX_ENGINE_DELTATIME);
          elapsed_time_us = (HAL_GetTick() - prev_tick) * 1000;
          BSP_LED_Off(LED2);
  
          if (Enabled6X == 1U)
          {
            (void)memcpy(&Msg->Data[55], (void *)pdata_out->quaternion_6X, 4U * sizeof(float));
            (void)memcpy(&Msg->Data[71], (void *)pdata_out->rotation_6X, 3U * sizeof(float));
            (void)memcpy(&Msg->Data[83], (void *)pdata_out->gravity_6X, 3U * sizeof(float));
            (void)memcpy(&Msg->Data[95], (void *)pdata_out->linear_acceleration_6X, 3U * sizeof(float));
  
            (void)memset(&Msg->Data[107], 0, sizeof(float));
            (void)memset(&Msg->Data[111], 0, sizeof(float));
          }
          else
          {
            (void)memcpy(&Msg->Data[55], (void *)pdata_out->quaternion_9X, 4U * sizeof(float));
            (void)memcpy(&Msg->Data[71], (void *)pdata_out->rotation_9X, 3U * sizeof(float));
            (void)memcpy(&Msg->Data[83], (void *)pdata_out->gravity_9X, 3U * sizeof(float));
            (void)memcpy(&Msg->Data[95], (void *)pdata_out->linear_acceleration_9X, 3U * sizeof(float));
  
            (void)memset(&Msg->Data[107], 0, sizeof(float));
            (void)memset(&Msg->Data[111], 0, sizeof(float));
          }
  
          Serialize_s32(&Msg->Data[115], (int32_t)elapsed_time_us, 4);
        }
      }
    }
  }
  
  /**
    * @brief  BSP Push Button callback
    * @param  Button Specifies the pin connected EXTI line
    * @retval None.
    */
  void BSP_PB_Callback(Button_TypeDef Button)
  {
    MagCalRequest = 1U;
  }
  
  /**
   * @brief  Handles the ACC axes data getting/sending
   * @param  Msg the ACC part of the stream
   * @retval None
   */
  static void Accelero_Sensor_Handler(TMsg *Msg)
  {
    if ((SensorsEnabled & ACCELEROMETER_SENSOR) == ACCELEROMETER_SENSOR)
    {
      if (UseOfflineData == 1)
      {
        AccValue.x = OfflineData[OfflineDataReadIndex].acceleration_x_mg;
        AccValue.y = OfflineData[OfflineDataReadIndex].acceleration_y_mg;
        AccValue.z = OfflineData[OfflineDataReadIndex].acceleration_z_mg;
      }
      else
      {
        BSP_SENSOR_ACC_GetAxes(&AccValue);
      }
  
      Serialize_s32(&Msg->Data[19], (int32_t)AccValue.x, 4);
      Serialize_s32(&Msg->Data[23], (int32_t)AccValue.y, 4);
      Serialize_s32(&Msg->Data[27], (int32_t)AccValue.z, 4);
    }
  }
  
  /**
   * @brief  Handles the GYR axes data getting/sending
   * @param  Msg the GYR part of the stream
   * @retval None
   */
  static void Gyro_Sensor_Handler(TMsg *Msg)
  {
    if ((SensorsEnabled & GYROSCOPE_SENSOR) == GYROSCOPE_SENSOR)
    {
      if (UseOfflineData == 1)
      {
        GyrValue.x = OfflineData[OfflineDataReadIndex].angular_rate_x_mdps;
        GyrValue.y = OfflineData[OfflineDataReadIndex].angular_rate_y_mdps;
        GyrValue.z = OfflineData[OfflineDataReadIndex].angular_rate_z_mdps;
      }
      else
      {
        BSP_SENSOR_GYR_GetAxes(&GyrValue);
      }
  
      Serialize_s32(&Msg->Data[31], GyrValue.x, 4);
      Serialize_s32(&Msg->Data[35], GyrValue.y, 4);
      Serialize_s32(&Msg->Data[39], GyrValue.z, 4);
    }
  }
  
  /**
   * @brief  Handles the MAG axes data getting/sending
   * @param  Msg the MAG part of the stream
   * @retval None
   */
  static void Magneto_Sensor_Handler(TMsg *Msg)
  {
    float ans_float;
    MFX_CM0P_MagCal_input_t mag_data_in;
    MFX_CM0P_MagCal_output_t mag_data_out;
  
    if ((SensorsEnabled & MAGNETIC_SENSOR) == MAGNETIC_SENSOR)
    {
      if (UseOfflineData == 1)
      {
       MagValue.x = OfflineData[OfflineDataReadIndex].magnetic_field_x_mgauss;
       MagValue.y = OfflineData[OfflineDataReadIndex].magnetic_field_y_mgauss;
       MagValue.z = OfflineData[OfflineDataReadIndex].magnetic_field_z_mgauss;
      }
      else
      {
        BSP_SENSOR_MAG_GetAxes(&MagValue);
  
        if (MagCalStatus == 0U)
        {
          mag_data_in.Mag[0] = (float)MagValue.x * FROM_MGAUSS_TO_UT50;
          mag_data_in.Mag[1] = (float)MagValue.y * FROM_MGAUSS_TO_UT50;
          mag_data_in.Mag[2] = (float)MagValue.z * FROM_MGAUSS_TO_UT50;
  
          MotionFX_manager_MagCal_run(&mag_data_in, &mag_data_out);
  
          if (mag_data_out.CalQuality == MFX_CM0P_CALQSTATUSBEST)
          {
            MagCalStatus = 1;
  
            ans_float = (mag_data_out.HI_Bias[0] * FROM_UT50_TO_MGAUSS);
            MagOffset.x = (int32_t)ans_float;
            ans_float = (mag_data_out.HI_Bias[1] * FROM_UT50_TO_MGAUSS);
            MagOffset.y = (int32_t)ans_float;
            ans_float = (mag_data_out.HI_Bias[2] * FROM_UT50_TO_MGAUSS);
            MagOffset.z = (int32_t)ans_float;
  
            /* Disable magnetometer calibration */
            MotionFX_manager_MagCal_stop(ALGO_PERIOD);
          }
        }
  
        MagValue.x = (int32_t)(MagValue.x - MagOffset.x);
        MagValue.y = (int32_t)(MagValue.y - MagOffset.y);
        MagValue.z = (int32_t)(MagValue.z - MagOffset.z);
      }
  
      Serialize_s32(&Msg->Data[43], MagValue.x, 4);
      Serialize_s32(&Msg->Data[47], MagValue.y, 4);
      Serialize_s32(&Msg->Data[51], MagValue.z, 4);
    }
  }
  
  /**
   * @brief  Handles the PRESS sensor data getting/sending.
   * @param  Msg the PRESS part of the stream
   * @retval None
   */
  static void Pressure_Sensor_Handler(TMsg *Msg)
  {
    if ((SensorsEnabled & PRESSURE_SENSOR) == PRESSURE_SENSOR)
    {
      if (UseOfflineData == 1)
      {
        PressValue = OfflineData[OfflineDataReadIndex].pressure;
      }
      else
      {
        BSP_SENSOR_PRESS_GetValue(&PressValue);
      }
  
      (void)memcpy(&Msg->Data[7], (void *)&PressValue, sizeof(float));
    }
  }
  
  /**
   * @brief  Handles the TEMP axes data getting/sending
   * @param  Msg the TEMP part of the stream
   * @retval None
   */
  static void Temperature_Sensor_Handler(TMsg *Msg)
  {
    if ((SensorsEnabled & TEMPERATURE_SENSOR) == TEMPERATURE_SENSOR)
    {
      if (UseOfflineData == 1)
      {
        TempValue = OfflineData[OfflineDataReadIndex].temperature;
      }
      else
      {
        BSP_SENSOR_TEMP_GetValue(&TempValue);
      }
  
      (void)memcpy(&Msg->Data[11], (void *)&TempValue, sizeof(float));
    }
  }
  
  /**
   * @brief  Handles the HUM axes data getting/sending
   * @param  Msg the HUM part of the stream
   * @retval None
   */
  static void Humidity_Sensor_Handler(TMsg *Msg)
  {
    if ((SensorsEnabled & HUMIDITY_SENSOR) == HUMIDITY_SENSOR)
    {
      if (UseOfflineData == 1)
      {
        HumValue = OfflineData[OfflineDataReadIndex].humidity;
      }
      else
      {
        BSP_SENSOR_HUM_GetValue(&HumValue);
      }
  
      (void)memcpy(&Msg->Data[15], (void *)&HumValue, sizeof(float));;
    }
  }
  
  /**
   * @brief  Timer configuration
   * @param  Freq the desired Timer frequency
   * @retval None
   */
  static void TIM_Config(uint32_t Freq)
  {
    const uint32_t tim_counter_clock = 2000; /* TIM counter clock 2 kHz */
    uint32_t prescaler_value = (uint32_t)((SystemCoreClock / tim_counter_clock) - 1);
    uint32_t period = (tim_counter_clock / Freq) - 1;
  
    BSP_IP_TIM_Handle.Init.Prescaler = prescaler_value;
    BSP_IP_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
    BSP_IP_TIM_Handle.Init.Period = period;
    BSP_IP_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    BSP_IP_TIM_Handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    if (HAL_TIM_Base_Init(&BSP_IP_TIM_Handle) != HAL_OK)
    {
      Error_Handler();
    }
  }
  
  #ifdef __cplusplus
  }
  #endif
  
  /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
  