FAQs
cancel
Turn on suggestions
Auto-suggest helps you quickly narrow down your search results by suggesting possible matches as you type.
Showing results for 
Search instead for 
Did you mean: 

Code Review Request: Integrating MotionFX with ASM330LHH, LIS2DH12, and LIS3MDL over SPI

leventdemirel99
Visitor

‎2025-03-06 10:32 AM

I tried to obtain roll pitch and yaw with the help of motionfx in the x-cube mems1 package by reading data with spi using ASM330LHH, LIS3MDL and LIS2DH12 sensors. I cannot try my code because I do not have the sensors available at the moment. Can you review the code, is there a faulty or missing part? I would appreciate if you can help me. I present the CS pin settings and the project in the attachment      

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 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"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "motion_fx.h"         // MotionFX API definitions (motion_fx.h file provided above)
#include "asm330lhh_reg.h"     // ASM330LHH sensor register functions
#include "lis2dh12_reg.h"      // LIS2DH12 sensor register functions
#include "lis3mdl_reg.h"       // LIS3MDL sensor register functions
#include <stdio.h>
#include <string.h>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* Structure that holds the SPI and Chip Select (CS) information for each sensor */
typedef struct {
  SPI_HandleTypeDef *hspi;
  GPIO_TypeDef     *CS_Port;
  uint16_t          CS_Pin;
} sensor_ctx_t;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* Set the buffer size for the MotionFX state memory. (Ensure it is large enough) */
#define MFX_STATE_BUFFER_SIZE 256
#define BOOT_TIME 10  // Boot time (in ms)
#define PWM_3V3 915
static uint8_t mfx_state_buffer[MFX_STATE_BUFFER_SIZE];

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* Global sensor context objects */
sensor_ctx_t sensor_ctx_imu; // ASM330LHH (SPI1)
sensor_ctx_t sensor_ctx_acc; // LIS2DH12 (SPI2)
sensor_ctx_t sensor_ctx_mag; // LIS3MDL (SPI3)

/* Sensor device contexts (ST sensor libraries) */
stmdev_ctx_t dev_ctx_imu;  // ASM330LHH (gyroscope only)
stmdev_ctx_t dev_ctx_acc;  // LIS2DH12 (accelerometer)
stmdev_ctx_t dev_ctx_mag;  // LIS3MDL (magnetometer)

uint8_t whoamI, rst;

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
SPI_HandleTypeDef hspi1;
SPI_HandleTypeDef hspi2;
SPI_HandleTypeDef hspi3;

TIM_HandleTypeDef htim3;

UART_HandleTypeDef huart1;

/* USER CODE BEGIN PV */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_SPI1_Init(void);
static void MX_SPI2_Init(void);
static void MX_SPI3_Init(void);
static void MX_TIM3_Init(void);
/* USER CODE BEGIN PFP */
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len);
static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len);
static void tx_com(uint8_t *tx_buffer, uint16_t len);
static void platform_delay(uint32_t ms);
static void platform_init(void);

void Sensors_Init(void);
void read_sensors(float gyro[3], float acc[3], float mag[3]);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* 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();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART1_UART_Init();
  MX_SPI1_Init();
  MX_SPI2_Init();
  MX_SPI3_Init();
  MX_TIM3_Init();
  /* USER CODE BEGIN 2 */

  	  platform_init();  // Sensor power supply via PWM outputs

    /* Transfer SPI and CS information to the sensor contexts */
    sensor_ctx_imu.hspi   = &hspi1;
    sensor_ctx_imu.CS_Port = CS_IMU_GPIO_Port;  // ASM330LHH CS port defined in the project
    sensor_ctx_imu.CS_Pin  = CS_IMU_Pin;         // ASM330LHH CS pin defined in the project

    sensor_ctx_acc.hspi   = &hspi2;
    sensor_ctx_acc.CS_Port = CS_ACC_GPIO_Port;  // For LIS2DH12
    sensor_ctx_acc.CS_Pin  = CS_ACC_Pin;

    sensor_ctx_mag.hspi   = &hspi3;
    sensor_ctx_mag.CS_Port = CS_MAG_GPIO_Port;  // For LIS3MDL
    sensor_ctx_mag.CS_Pin  = CS_MAG_Pin;

    /* Initialize sensors */
    Sensors_Init();

    /* Initialize the MotionFX state memory buffer and engine state */
    size_t state_size = MotionFX_GetStateSize();
    if (state_size > MFX_STATE_BUFFER_SIZE) { Error_Handler(); }
    MFXState_t mfx_state = (MFXState_t)mfx_state_buffer;
    MotionFX_initialize(mfx_state);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
	  	  	  float raw_gyro[3] = {0}; // ASM330LHH gyroscope data [mdps]
	 	      float raw_acc[3]  = {0}; // LIS2DH12 accelerometer data [mg]
	 	      float raw_mag[3]  = {0}; // LIS3MDL magnetometer data [mG]

	 	      read_sensors(raw_gyro, raw_acc, raw_mag);

	 	      /* Conversions:
	 	         - Gyro: mdps -> dps (divide by 1000)
	 	         - Acc: mg -> g (divide by 1000)
	 	         - Mag: mG -> [uT/50] : (mG * 10 gives microTesla, then divide by 50 = mG * 0.2) */
	 	      MFX_input_t mfx_input;
	 	      for (int i = 0; i < MFX_NUM_AXES; i++) {
	 	        mfx_input.gyro[i] = raw_gyro[i] / 1000.0f;  // [dps]
	 	        mfx_input.acc[i]  = raw_acc[i]  / 1000.0f;   // [g]
	 	        mfx_input.mag[i]  = raw_mag[i]  * 0.2f;        // [uT/50]
	 	      }

	 	      float dt = 0.01f;      // Delta time [sec] (e.g. 10ms)
	 	      float q_update = 0.0f; // If needed, we leave it as 0 here

	 	      MFX_output_t mfx_output;
	 	      /* Run the MotionFX algorithm */
	 	      MotionFX_update(mfx_state, &mfx_output, &mfx_input, &dt, &q_update);
	 	      /* Note: As a result of the fusion algorithm, MotionFX_update() places the angle values
	 	               [yaw, pitch, roll] in the mfx_output.rotation[] array. */

	 	      /* Send the result over UART */
	 	      char tx_buffer[100];
	 	      snprintf(tx_buffer, sizeof(tx_buffer),
	 	               "Yaw: %f, Pitch: %f, Roll: %f\r\n",
	 	               mfx_output.rotation[0],
	 	               mfx_output.rotation[1],
	 	               mfx_output.rotation[2]);
	 	      tx_com((uint8_t*)tx_buffer, strlen(tx_buffer));

	 	      HAL_Delay(10);
  }
  /* 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 the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __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_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 150;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 3;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = 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_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @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_8;
  hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi1.Init.CRCPolynomial = 10;
  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_8BIT;
  hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
  hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
  hspi2.Init.NSS = SPI_NSS_SOFT;
  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
  hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi2.Init.CRCPolynomial = 10;
  if (HAL_SPI_Init(&hspi2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI2_Init 2 */

  /* USER CODE END SPI2_Init 2 */

}

/**
  * @brief SPI3 Initialization Function
  * @PAram None
  * @retval None
  */
static void MX_SPI3_Init(void)
{

  /* USER CODE BEGIN SPI3_Init 0 */

  /* USER CODE END SPI3_Init 0 */

  /* USER CODE BEGIN SPI3_Init 1 */

  /* USER CODE END SPI3_Init 1 */
  /* SPI3 parameter configuration*/
  hspi3.Instance = SPI3;
  hspi3.Init.Mode = SPI_MODE_MASTER;
  hspi3.Init.Direction = SPI_DIRECTION_2LINES;
  hspi3.Init.DataSize = SPI_DATASIZE_8BIT;
  hspi3.Init.CLKPolarity = SPI_POLARITY_HIGH;
  hspi3.Init.CLKPhase = SPI_PHASE_2EDGE;
  hspi3.Init.NSS = SPI_NSS_SOFT;
  hspi3.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
  hspi3.Init.FirstBit = SPI_FIRSTBIT_MSB;
  hspi3.Init.TIMode = SPI_TIMODE_DISABLE;
  hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
  hspi3.Init.CRCPolynomial = 10;
  if (HAL_SPI_Init(&hspi3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN SPI3_Init 2 */

  /* USER CODE END SPI3_Init 2 */

}

/**
  * @brief TIM3 Initialization Function
  * @PAram None
  * @retval None
  */
static void MX_TIM3_Init(void)
{

  /* USER CODE BEGIN TIM3_Init 0 */

  /* USER CODE END TIM3_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM3_Init 1 */

  /* USER CODE END TIM3_Init 1 */
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 0;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 65535;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &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(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM3_Init 2 */

  /* USER CODE END TIM3_Init 2 */
  HAL_TIM_MspPostInit(&htim3);

}

/**
  * @brief USART1 Initialization Function
  * @PAram None
  * @retval None
  */
static void MX_USART1_UART_Init(void)
{

  /* USER CODE BEGIN USART1_Init 0 */

  /* USER CODE END USART1_Init 0 */

  /* USER CODE BEGIN USART1_Init 1 */

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */

  /* USER CODE END USART1_Init 2 */

}

/**
  * @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_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOE_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, CS_IMU_Pin|CS_ACC_Pin, GPIO_PIN_SET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(CS_MAG_GPIO_Port, CS_MAG_Pin, GPIO_PIN_SET);

  /*Configure GPIO pins : CS_IMU_Pin CS_ACC_Pin */
  GPIO_InitStruct.Pin = CS_IMU_Pin|CS_ACC_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pin : CS_MAG_Pin */
  GPIO_InitStruct.Pin = CS_MAG_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
  HAL_GPIO_Init(CS_MAG_GPIO_Port, &GPIO_InitStruct);

/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}

/* USER CODE BEGIN 4 */
/* =======================================================================
                         Sensor Initialization Function
   ======================================================================= */
void Sensors_Init(void)
{
  /* --- ASM330LHH (Gyroscope Only) - SPI1 --- */
  dev_ctx_imu.write_reg = platform_write;
  dev_ctx_imu.read_reg  = platform_read;
  dev_ctx_imu.mdelay    = platform_delay;
  dev_ctx_imu.handle    = &sensor_ctx_imu;  // ASM330LHH: SPI1 & associated CS

  platform_delay(BOOT_TIME);
  asm330lhh_device_id_get(&dev_ctx_imu, &whoamI);
  if (whoamI != ASM330LHH_ID) { Error_Handler(); }
  asm330lhh_reset_set(&dev_ctx_imu, PROPERTY_ENABLE);
  do { asm330lhh_reset_get(&dev_ctx_imu, &rst); } while (rst);
  asm330lhh_device_conf_set(&dev_ctx_imu, PROPERTY_ENABLE);
  asm330lhh_block_data_update_set(&dev_ctx_imu, PROPERTY_ENABLE);
  asm330lhh_gy_data_rate_set(&dev_ctx_imu, ASM330LHH_GY_ODR_12Hz5);
  asm330lhh_gy_full_scale_set(&dev_ctx_imu, ASM330LHH_2000dps);

  /* --- LIS2DH12 (Accelerometer) - SPI2 --- */
  dev_ctx_acc.write_reg = platform_write;
  dev_ctx_acc.read_reg  = platform_read;
  dev_ctx_acc.mdelay    = platform_delay;
  dev_ctx_acc.handle    = &sensor_ctx_acc;  // LIS2DH12: SPI2 & associated CS

  platform_delay(BOOT_TIME);
  lis2dh12_device_id_get(&dev_ctx_acc, &whoamI);
  if (whoamI != LIS2DH12_ID) { Error_Handler(); }
  lis2dh12_block_data_update_set(&dev_ctx_acc, PROPERTY_ENABLE);
  lis2dh12_data_rate_set(&dev_ctx_acc, LIS2DH12_ODR_10Hz);
  lis2dh12_full_scale_set(&dev_ctx_acc, LIS2DH12_2g);
  lis2dh12_temperature_meas_set(&dev_ctx_acc, LIS2DH12_TEMP_ENABLE);
  lis2dh12_operating_mode_set(&dev_ctx_acc, LIS2DH12_HR_12bit);

  /* --- LIS3MDL (Magnetometer) - SPI3 --- */
  dev_ctx_mag.write_reg = platform_write;
  dev_ctx_mag.read_reg  = platform_read;
  dev_ctx_mag.mdelay    = platform_delay;
  dev_ctx_mag.handle    = &sensor_ctx_mag;  // LIS3MDL: SPI3 & associated CS

  platform_delay(BOOT_TIME);
  lis3mdl_device_id_get(&dev_ctx_mag, &whoamI);
  if (whoamI != LIS3MDL_ID) { Error_Handler(); }
  lis3mdl_reset_set(&dev_ctx_mag, PROPERTY_ENABLE);
  do { lis3mdl_reset_get(&dev_ctx_mag, &rst); } while (rst);
  lis3mdl_block_data_update_set(&dev_ctx_mag, PROPERTY_ENABLE);
  lis3mdl_data_rate_set(&dev_ctx_mag, LIS3MDL_HP_1Hz25);
  lis3mdl_full_scale_set(&dev_ctx_mag, LIS3MDL_16_GAUSS);
  lis3mdl_temperature_meas_set(&dev_ctx_mag, PROPERTY_ENABLE);
  lis3mdl_operating_mode_set(&dev_ctx_mag, LIS3MDL_CONTINUOUS_MODE);
}

/* =======================================================================
                         Sensor Data Reading Function
   ======================================================================= */
void read_sensors(float gyro[3], float acc[3], float mag[3])
{
  uint8_t flag;

  /* --- ASM330LHH (Gyroscope - SPI1) --- */
  int16_t raw_gyro[3] = {0};
  asm330lhh_gy_flag_data_ready_get(&dev_ctx_imu, &flag);
  if (flag)
  {
    asm330lhh_angular_rate_raw_get(&dev_ctx_imu, raw_gyro);
    for (int i = 0; i < 3; i++)
    {
      gyro[i] = raw_gyro[i];  // mdps; conversion will be done in main()
    }
  }

  /* --- LIS2DH12 (Accelerometer - SPI2) --- */
  int16_t raw_acc[3] = {0};
  lis2dh12_reg_t lis2dh_flag;
  lis2dh12_xl_data_ready_get(&dev_ctx_acc, &lis2dh_flag.byte);
  if (lis2dh_flag.byte)
  {
    lis2dh12_acceleration_raw_get(&dev_ctx_acc, raw_acc);
    for (int i = 0; i < 3; i++)
    {
      acc[i] = raw_acc[i];  // mg; conversion will be done in main()
    }
  }

  /* --- LIS3MDL (Magnetometer - SPI3) --- */
  int16_t raw_mag[3] = {0};
  uint8_t mag_flag;
  lis3mdl_mag_data_ready_get(&dev_ctx_mag, &mag_flag);
  if (mag_flag)
  {
    lis3mdl_magnetic_raw_get(&dev_ctx_mag, raw_mag);
    for (int i = 0; i < 3; i++)
    {
      mag[i] = raw_mag[i];  // mG; conversion will be done in main()
    }
  }
}

/* =======================================================================
                         Platform Dependent Functions
   ======================================================================= */
static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len)
{
  sensor_ctx_t *ctx = (sensor_ctx_t *)handle;
  HAL_GPIO_WritePin(ctx->CS_Port, ctx->CS_Pin, GPIO_PIN_RESET);
  HAL_SPI_Transmit(ctx->hspi, &reg, 1, 1000);
  HAL_SPI_Transmit(ctx->hspi, (uint8_t*)bufp, len, 1000);
  HAL_GPIO_WritePin(ctx->CS_Port, ctx->CS_Pin, GPIO_PIN_SET);
  return 0;
}

static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len)
{
  sensor_ctx_t *ctx = (sensor_ctx_t *)handle;
  reg |= 0x80;
  HAL_GPIO_WritePin(ctx->CS_Port, ctx->CS_Pin, GPIO_PIN_RESET);
  HAL_SPI_Transmit(ctx->hspi, &reg, 1, 1000);
  HAL_SPI_Receive(ctx->hspi, bufp, len, 1000);
  HAL_GPIO_WritePin(ctx->CS_Port, ctx->CS_Pin, GPIO_PIN_SET);
  return 0;
}

static void tx_com(uint8_t *tx_buffer, uint16_t len)
{
  HAL_UART_Transmit(&huart1, tx_buffer, len, 1000);
}

static void platform_delay(uint32_t ms)
{
  HAL_Delay(ms);
}

/* For PWM outputs and sensor power supply */
static void platform_init(void)
{
  TIM3->CCR1 = PWM_3V3;
  TIM3->CCR2 = PWM_3V3;
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
  HAL_Delay(1000);
}
/* USER CODE END 4 */

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

 

Preview file
26 KB
Preview file
23 KB
ASM_LIS2_LIS3_withFX.zip
1 REPLY 1
alperenbolat66
Visitor

‎2025-03-06 10:59 AM

I wanna learn to answer for this question ! 

0 Kudos
Top