BNO085 I2C with STM32H7A3 via SH2 library — stuck at 44Hz but ESP32 achieves 1.2ms read time

[dinesh_ee_bme_72]

Hi everyone,

I am working on a project using BNO085 IMU sensor and STM32H7A3ZIT6Q. I have a single sensor working but stuck at 44Hz. The same sensor with SparkFun library on ESP32 achieves 1.2ms read time. I need help understanding why and how to match ESP32 performance on STM32.

Hardware:

• NUCLEO-H7A3ZI-Q (STM32H7A3ZIT6Q, Cortex-M7 @ 280MHz)
• Adafruit BNO085 breakout board
• I2C on PB8 (SCL), PB9 (SDA), PC0 (INT pin)
• P0 and P1 tied to GND (I2C mode, address 0x4A)

Software:

• STM32CubeIDE with HAL
• Official CEVA SH2 library from github.com/ceva-dsp/sh2
• REPORT_INTERVAL_US = 2500 (targeting 400Hz)
• SH2_ROTATION_VECTOR (9-axis full quaternion)

STM32 performance achieved so far:

• I2C 400kHz polling: 28Hz, read_ms = 32ms
• I2C 1MHz Fast Mode Plus: 44Hz, read_ms = 19ms
• INT pin wait inside sh2_hal_read(): still 44Hz, read_ms = 19ms

ESP32 performance with same sensor and SparkFun SH2 library:

• read time: 1.2ms
• Data rate: close to 400Hz

This is a huge difference. 19ms on STM32 vs 1.2ms on ESP32 with the same SH2 library underneath.

Current STM32 sh2_hal_read implementation:

static int sh2_hal_read( sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us ) 
{
 uint32_t timeout = HAL_GetTick() + 100;
 while (HAL_GPIO_ReadPin(GPIOC, GPIO_PIN_0) == GPIO_PIN_SET)
 {
  if (HAL_GetTick() > timeout) return 0;
 }
 HAL_StatusTypeDef status;
 status = HAL_I2C_Master_Receive( &hi2c1, BNO085_ADDR, pBuffer, len, 100 );
 if( status != HAL_OK ) return SH2_ERR_IO;
 *t_us = HAL_GetTick() * 1000;
 return len; 
}

Key observation: ESP32 SparkFun library reads 4 byte header first, then reads payload in 32 byte chunks with INT pin check between each chunk. Our STM32 reads entire packet in one HAL_I2C_Master_Receive call. Not sure if this is the cause of the difference.

What I have tried:

1. I2C 400kHz to 1MHz Fast Mode Plus — improved from 28Hz to 44Hz
2. INT pin wait inside sh2_hal_read() — no improvement
3. Direct pin polling in main loop — no improvement

Questions:

1. Why does ESP32 achieve 1.2ms at 400kHz I2C while STM32 takes 19ms at 1MHz?
2. Is the bottleneck in HAL_I2C_Master_Receive vs ESP32 Wire implementation?
3. Has anyone implemented a proper sh2_hal_read() for STM32H7 that achieves fast read times?
4. Has anyone used I2C DMA with SH2 library on STM32H7? What improvement did you see?
5. Is SPI the only reliable path to 200Hz+?

---

Edited to apply source code formatting - please see How to insert source code for future reference.

[Andrew Neil]

You've marked the thread as solved - ie, answered ?

[dinesh_ee_bme_72]

yes same issue that 44hz with stm I still did not get the solution 

[Andrew Neil]

So merged into your original thread.

The topic wasn't even the same - the other thread was on a different board!

---

@dinesh_ee_bme_72 wrote:

 I still did not get the solution 

---

So why did you mark this thread as solved ?

Instructions to unmark it are here.

[dinesh_ee_bme_72]

unmarked done

[Andrew Neil]

So what is the status now?

Have you got your basic I2C comms working?

What problem(s) exactly are you still facing?

[dinesh_ee_bme_72]

I am working on reading quaternion data from a BNO085 IMU sensor connected to an STM32H7A3ZIT6Q (NUCLEO-H7A3ZI-Q) over I2C1 at 1MHz Fast Mode Plus using the CEVA SH2 library. Here is my situation:

Setup:

• MCU: STM32H7A3ZIT6Q, Cortex-M7 @ 280MHz, NUCLEO-H7A3ZI-Q
• Sensor: BNO085 Adafruit breakout, I2C address 0x4A
• Interface: I2C1, Fast Mode Plus, 1MHz,
• Protocol: SHTP via CEVA SH2 library
• INT pin connected to PC0 with EXTI falling edge interrupt

What is happening:

• Sensor initializes correctly every time — "Sensor initialized OK!" prints
• No quaternion data ever appears after the header line
• Board reset does not help — same result every time

What I have already tried:

• Upgraded I2C from 400kHz to 1MHz — improved rate from 28Hz to 44Hz
• Fixed interrupt usage — sh2_service() was being called unconditionally every loop iteration ignoring nint_triggered flag — fixing this improved rate to 52Hz
• Added nint_triggered = true after initBNO085() to break deadlock caused by missed INT pulses during BNO085 boot sequence
• Changed from single sh2_service() call to do-while loop calling sh2_service() until INT pin goes HIGH to fully drain SHTP buffer — after this change output stopped completely

as bNO update rate is 400hz for rotation vector so at least  I am expecting 200hz for my application what further steps I can take ? 

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : BNO085 Quaternion Reader with Timing
  * @author         : Dinii @ IIT Madras
  ******************************************************************************
  */
/* USER CODE END Header */
#include "main.h"

/* USER CODE BEGIN Includes */
#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include "sh2.h"
#include "sh2_SensorValue.h"
#include "sh2_err.h"
/* USER CODE END Includes */

/* USER CODE BEGIN PD */
#define BNO085_ADDR         (0x4A << 1)
#define REPORT_INTERVAL_US  2500
/* USER CODE END PD */

I2C_HandleTypeDef hi2c1; 
UART_HandleTypeDef huart3;

/* USER CODE BEGIN PV */
static sh2_Hal_t sh2_hal;
static sh2_SensorValue_t sensorValue;
static bool sensorReady = false;
static bool newDataAvailable = false;
volatile bool nint_triggered = false;
static uint32_t sampleCount = 0;
/* USER CODE END PV */

void SystemClock_Config(void);
static void MPU_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_USART3_UART_Init(void);

/* USER CODE BEGIN PFP */
static int sh2_hal_open(sh2_Hal_t *self);
static void sh2_hal_close(sh2_Hal_t *self);
static int sh2_hal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us);
static int sh2_hal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len);
static uint32_t sh2_hal_getTimeUs(sh2_Hal_t *self);
static void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent);
static void eventHandler(void *cookie, sh2_AsyncEvent_t *pEvent);
/* USER CODE END PFP */

/* USER CODE BEGIN 0 */

static int sh2_hal_open(sh2_Hal_t *self)
{
    return SH2_OK;
}

static void sh2_hal_close(sh2_Hal_t *self)
{
}

/* ---------------------------------------------------------------
   CHANGE 1 — sh2_hal_read: reduced timeout from 100ms to 10ms
              changed return 0 to break so I2C read still attempts
   REASON:    SHTP calls sh2_hal_read multiple times per sample
              (once for header, once for payload, sometimes more)
              Between these internal calls INT may not fire again
              Hard return 0 was causing SH2 library to abort
              break lets the I2C read attempt even if INT is slow
   --------------------------------------------------------------- */
static int sh2_hal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len, uint32_t *t_us)
{
    uint32_t timeout = HAL_GetTick() + 10;
    while (HAL_GPIO_ReadPin(GPIOC, GPIO_PIN_0) == GPIO_PIN_SET)
    {
        if (HAL_GetTick() > timeout) break;  // break not return
    }
    HAL_StatusTypeDef status;
    status = HAL_I2C_Master_Receive(&hi2c1, BNO085_ADDR, pBuffer, len, 100);
    if (status != HAL_OK) return SH2_ERR_IO;
    *t_us = HAL_GetTick() * 1000;
    return len;
}

static int sh2_hal_write(sh2_Hal_t *self, uint8_t *pBuffer, unsigned len)
{
    HAL_StatusTypeDef status;
    status = HAL_I2C_Master_Transmit(&hi2c1, BNO085_ADDR, pBuffer, len, 100);
    if (status != HAL_OK) return SH2_ERR_IO;
    return len;
}

static uint32_t sh2_hal_getTimeUs(sh2_Hal_t *self)
{
    return HAL_GetTick() * 1000;
}

static void eventHandler(void *cookie, sh2_AsyncEvent_t *pEvent)
{
    if (pEvent->eventId == SH2_RESET)
    {
        sensorReady = false;
    }
}

static void sensorHandler(void *cookie, sh2_SensorEvent_t *pEvent)
{
    int rc = sh2_decodeSensorEvent(&sensorValue, pEvent);
    if (rc == SH2_OK)
    {
        if (sensorValue.sensorId == SH2_ROTATION_VECTOR)
        {
            newDataAvailable = true;
        }
    }
}

static void uart_print(const char *str)
{
    HAL_UART_Transmit(&huart3, (uint8_t*)str, strlen(str), 100);
}

static bool initBNO085(void)
{
    int rc;
    char msg[100];

    uart_print("\r\n=== BNO085 Quaternion Reader ===\r\n");
    uart_print("Initializing sensor...\r\n");

    sh2_hal.open      = sh2_hal_open;
    sh2_hal.close     = sh2_hal_close;
    sh2_hal.read      = sh2_hal_read;
    sh2_hal.write     = sh2_hal_write;
    sh2_hal.getTimeUs = sh2_hal_getTimeUs;

    rc = sh2_open(&sh2_hal, eventHandler, NULL);
    if (rc != SH2_OK)
    {
        sprintf(msg, "sh2_open failed: %d\r\n", rc);
        uart_print(msg);
        return false;
    }

    rc = sh2_setSensorCallback(sensorHandler, NULL);
    if (rc != SH2_OK)
    {
        sprintf(msg, "sh2_setSensorCallback failed: %d\r\n", rc);
        uart_print(msg);
        return false;
    }

    sh2_SensorConfig_t config;
    memset(&config, 0, sizeof(config));
    config.reportInterval_us = REPORT_INTERVAL_US;
    config.changeSensitivityEnabled = false;
    config.changeSensitivityRelative = false;
    config.wakeupEnabled = false;
    config.alwaysOnEnabled = false;

    rc = sh2_setSensorConfig(SH2_ROTATION_VECTOR, &config);
    if (rc != SH2_OK)
    {
        sprintf(msg, "sh2_setSensorConfig failed: %d\r\n", rc);
        uart_print(msg);
        return false;
    }

    uart_print("Sensor initialized OK!\r\n");
    uart_print("\r\nFormat: t_ms,dt_ms,read_ms,W,X,Y,Z\r\n\r\n");

    sensorReady = true;
    return true;
}

/* USER CODE END 0 */

int main(void)
{
  /* USER CODE BEGIN 1 */
  char msg[150];
  uint32_t t_now, t_prev, dt, read_start, read_time;
  float W, X, Y, Z;
  /* USER CODE END 1 */

  MPU_Config();
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART3_UART_Init();

  /* USER CODE BEGIN 2 */
  HAL_Delay(500);

  if (!initBNO085())
    {
        uart_print("ERROR: Failed to initialize BNO085!\r\n");
        uart_print("Check: P0->GND, P1->GND, SDA->PB9, SCL->PB8\r\n");
        while(1) { HAL_Delay(1000); }
    }

    nint_triggered = true;   // kickstart first read after init

    t_prev = HAL_GetTick();
  /* USER CODE END 2 */

  /* USER CODE BEGIN WHILE */
  while (1)
  {
      if (!sensorReady)
      {
          HAL_Delay(100);
          initBNO085();
          t_prev = HAL_GetTick();
      }

      /* ---------------------------------------------------------------
         CHANGE 2 — main loop: sh2_service() now inside nint_triggered
         REASON:    before this change sh2_service() was called every
                    single loop iteration wasting CPU
                    now it only runs when INT pin has actually fired
                    nint_triggered is set by EXTI callback below
         --------------------------------------------------------------- */
      if (nint_triggered)
            {
                nint_triggered = false;

                read_start = HAL_GetTick();
                do
                {
                    sh2_service();
                }
                while (HAL_GPIO_ReadPin(GPIOC, GPIO_PIN_0) == GPIO_PIN_RESET);
                read_time = HAL_GetTick() - read_start;

          if (newDataAvailable)
          {
              newDataAvailable = false;
              sampleCount++;
              t_now = HAL_GetTick();
              dt = t_now - t_prev;
              t_prev = t_now;
              W = sensorValue.un.rotationVector.real;
              X = sensorValue.un.rotationVector.i;
              Y = sensorValue.un.rotationVector.j;
              Z = sensorValue.un.rotationVector.k;
              sprintf(msg, "%lu,%lu,%lu,%.4f,%.4f,%.4f,%.4f\r\n",
                      t_now, dt, read_time, W, X, Y, Z);
              uart_print(msg);
          }
      }

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/* USER CODE BEGIN 4 */
/* ---------------------------------------------------------------
   CHANGE 3 — EXTI callback: no code change, was already correct
   REASON:    documenting here for clarity
              this fires automatically when INT pin goes LOW
              sets nint_triggered = true
              main loop above now reads and uses this flag
   --------------------------------------------------------------- */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    if (GPIO_Pin == GPIO_PIN_0)
    {
        nint_triggered = true;
    }
}
/* USER CODE END 4 */

void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC->CKGAENR = 0xE003FFFF;
  HAL_PWREx_ConfigSupply(PWR_DIRECT_SMPS_SUPPLY);
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
  while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_DIV1;
  RCC_OscInitStruct.HSICalibrationValue = 64;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 4;
  RCC_OscInitStruct.PLL.PLLN = 35;
  RCC_OscInitStruct.PLL.PLLP = 2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  RCC_OscInitStruct.PLL.PLLR = 2;
  RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
  RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
  RCC_OscInitStruct.PLL.PLLFRACN = 0;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) Error_Handler();
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
                              |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
  RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_6) != HAL_OK) Error_Handler();
}

static void MX_I2C1_Init(void)
{
  hi2c1.Instance = I2C1;
  hi2c1.Init.Timing = 0x00601C5C;
  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();
  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK) Error_Handler();
  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK) Error_Handler();
  HAL_I2CEx_EnableFastModePlus(I2C_FASTMODEPLUS_I2C1);
}

static void MX_USART3_UART_Init(void)
{
  huart3.Instance = USART3;
  huart3.Init.BaudRate = 115200;
  huart3.Init.WordLength = UART_WORDLENGTH_8B;
  huart3.Init.StopBits = UART_STOPBITS_1;
  huart3.Init.Parity = UART_PARITY_NONE;
  huart3.Init.Mode = UART_MODE_TX_RX;
  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
  huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
  huart3.Init.ClockPrescaler = UART_PRESCALER_DIV1;
  huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
  if (HAL_UART_Init(&huart3) != HAL_OK) Error_Handler();
  if (HAL_UARTEx_SetTxFifoThreshold(&huart3, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) Error_Handler();
  if (HAL_UARTEx_SetRxFifoThreshold(&huart3, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) Error_Handler();
  if (HAL_UARTEx_DisableFifoMode(&huart3) != HAL_OK) Error_Handler();
}

static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
  GPIO_InitStruct.Pin = GPIO_PIN_0;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
  HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
  HAL_NVIC_EnableIRQ(EXTI0_IRQn);
}

void MPU_Config(void)
{
  MPU_Region_InitTypeDef MPU_InitStruct = {0};
  HAL_MPU_Disable();
  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
  MPU_InitStruct.BaseAddress = 0x0;
  MPU_InitStruct.Size = MPU_REGION_SIZE_4GB;
  MPU_InitStruct.SubRegionDisable = 0x87;
  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
  MPU_InitStruct.AccessPermission = MPU_REGION_NO_ACCESS;
  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
  MPU_InitStruct.IsShareable = MPU_ACCESS_SHAREABLE;
  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
  HAL_MPU_ConfigRegion(&MPU_InitStruct);
  HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
}

void Error_Handler(void)
{
  __disable_irq();
  while (1) {}
}

[Andrew Neil]

You didn't answer the question: Have you got your basic I2C comms working?

Have you confirmed that you can do basic reads & writes to & from the registers on the BNO085 ?

BTW:

Rather than:

    sh2_SensorConfig_t config;
    memset(&config, 0, sizeof(config));

You could just do:

    sh2_SensorConfig_t config ={0};

PS:

---

@dinesh_ee_bme_72In the OP,  wrote:

The same sensor with SparkFun library on ESP32 achieves 1.2ms read time

---

So have you compared what happens in that case against what happens in your STM32 case?

• Step through each in their respective debuggers
• Look at what's happening on the I2C wires in both cases

As suggested in the other thread:

are you sure that you have the correct I2C address and that you are presenting it correctly?

See this and this

[dinesh_ee_bme_72]

---

Basic I2C is confirmed working — sh2_open(), sh2_setSensorCallback(), and sh2_setSensorConfig() all return OK every time, which requires multiple successful I2C transactions. "Sensor initialized OK!" prints reliably.

Key observation: the original polling-based version worked at 44Hz continuously with no issues. After switching to interrupt-driven architecture with nint_triggered flag and do-while buffer drain, output stopped completely — newDataAvailable never becomes true.

So hardware and I2C are fine. The problem is specifically in how the interrupt-driven flow interacts with the SHTP boot sequence.

My goals are:

• Fix the interrupt-driven architecture so quaternions print continuously without deadlock
• Push sample rate toward 200Hz (currently stuck at 44Hz polling, sensor rated at 400Hz)

My questions:

• What is the correct interrupt-driven pattern for BNO085 on STM32 using sh2_service()?
• What is the realistic maximum rate achievable over I2C at 1MHz with SHTP?
• Is DMA on I2C the right next step for improving rate, or is SPI migration necessary to reach 200Hz?

[Andrew Neil]

So, again, have you used and oscilloscope and/or logic analyser to see what's happening on the wires in your non-working interrupt-driven architecture ?

And compare this to what happens in the polled architecture, and the ESP32 version.

---

@dinesh_ee_bme_72 wrote:

---

interrupt-driven architecture with nint_triggered flag 

---

Pardon ?