Hello Bosh Community,
I have a custom board embedding a Dual-Core STM32H7, with a bunch of sensors, including the BHI260AP and its auxiliary sensors (BMM150, BME688 and BMP390).
I'm struggling a bit after the initialization step. Here's the code snippet that i have :
/*
* sensor_fusion_cluster.c
*
* Created on: Jun 2, 2025
* Author: Yassine DEHHANI
*/
#include "drivers_h/bhi260_sensor_cluster/bhy2.h"
#include "drivers_h/bhi260_sensor_cluster/bhy2_parse.h"
#include "drivers_h/bhi260_sensor_cluster/common.h"
#include "sensors_hubs/sensor_fusion_cluster.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#define WORK_BUFFER_SIZE 2048
#define FIFO_BUFFER_SIZE 2048
#define SAMPLE_RATE_HZ 100.0f
#define REPORT_LATENCY 0
#define QUAT_SENSOR_ID BHY2_SENSOR_ID_GAMERV // Changed from GAMERV to RV_WU
#define RAD2DEG (180.0f / M_PI)
/*#define UPLOAD_FIRMWARE_TO_FLASH */
#ifdef UPLOAD_FIRMWARE_TO_FLASH
#include "drivers_h/bhi260_sensor_cluster/firmware/bhi260ap/BHI260AP_aux_BMM150_BMP390_BME688-flash.fw.h"
#else
#include "drivers_h/bhi260_sensor_cluster/firmware/bhi260ap/BHI260AP_aux_BMM150_BMP390_BME688.fw.h"
#endif
static struct bhy2_dev bhi260;
enum bhy2_intf intf;
// Align buffer to avoid possible DMA or hard fault issues
__attribute__((aligned(32))) static uint8_t work_buffer[WORK_BUFFER_SIZE];
static void parse_quaternion(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref)
{
(void)callback_ref;
// Defensive check
if (callback_info == NULL || callback_info->data_ptr == NULL)
{
printf("[ERROR] parse_quaternion: Null callback info or data_ptr\r\n");
return;
}
if (callback_info->data_size != 11)
{
printf("[WARN] Invalid quaternion size: %u\r\n", callback_info->data_size);
return;
}
struct bhy2_data_quaternion data;
bhy2_parse_quaternion(callback_info->data_ptr, &data);
uint64_t timestamp = *callback_info->time_stamp;
timestamp = timestamp * 15625;
uint32_t s = (uint32_t)(timestamp / UINT64_C(1000000000));
uint32_t ns = (uint32_t)(timestamp - (s * UINT64_C(1000000000)));
// Convert raw values to float
float qx = data.x / 16384.0f;
float qy = data.y / 16384.0f;
float qz = data.z / 16384.0f;
float qw = data.w / 16384.0f;
// Extract valid accuracy (low 2 bits)
uint8_t accuracy = data.accuracy & 0x03;
// float roll = atan2f(2.0f * (qw * qx + qy * qz), 1.0f - 2.0f * (qx * qx + qy * qy)) * RAD2DEG;
// float pitch = asinf(2.0f * (qw * qy - qz * qx)) * RAD2DEG;
// float yaw = atan2f(2.0f * (qw * qz + qx * qy), 1.0f - 2.0f * (qy * qy + qz * qz)) * RAD2DEG;
// Optional: convert timestamp to seconds + nanoseconds already done as `s` and `ns`
// Now you can break here and inspect qx, qy, qz, qw, accuracy
// Or later print if needed:
printf("SID: %u; T: %lu.%09lu; x: %f, y: %f, z: %f, w: %f; acc: %u\r\n",
callback_info->sensor_id,
s,
ns,
qx,
qy,
qz,
qw,
accuracy);
}
static void parse_meta_event(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref)
{
(void)callback_ref;
// Defensive check
if (callback_info == NULL || callback_info->data_ptr == NULL)
{
printf("[ERROR] parse_meta_event: Null callback info or data_ptr\r\n");
return;
}
if (callback_info->data_size < 3)
{
printf("[WARN] Invalid meta event size: %u\r\n", callback_info->data_size);
return;
}
uint8_t meta_event_type = callback_info->data_ptr[0];
uint8_t byte1 = callback_info->data_ptr[1];
uint8_t byte2 = callback_info->data_ptr[2];
char *event_text = (callback_info->sensor_id == BHY2_SYS_ID_META_EVENT) ? "[META EVENT]" : "[META EVENT WAKE UP]";
switch (meta_event_type)
{
case BHY2_META_EVENT_FLUSH_COMPLETE:
printf("%s Flush complete for sensor id %u\r\n", event_text, byte1);
break;
case BHY2_META_EVENT_SAMPLE_RATE_CHANGED:
printf("%s Sample rate changed for sensor id %u\r\n", event_text, byte1);
break;
case BHY2_META_EVENT_POWER_MODE_CHANGED:
printf("%s Power mode changed for sensor id %u\r\n", event_text, byte1);
break;
case BHY2_META_EVENT_ALGORITHM_EVENTS:
printf("%s Algorithm event\r\n", event_text);
break;
case BHY2_META_EVENT_SENSOR_STATUS:
printf("%s Accuracy for sensor id %u changed to %u\r\n", event_text, byte1, byte2);
break;
case BHY2_META_EVENT_BSX_DO_STEPS_MAIN:
printf("%s BSX event (do steps main)\r\n", event_text);
break;
case BHY2_META_EVENT_BSX_DO_STEPS_CALIB:
printf("%s BSX event (do steps calib)\r\n", event_text);
break;
case BHY2_META_EVENT_BSX_GET_OUTPUT_SIGNAL:
printf("%s BSX event (get output signal)\r\n", event_text);
break;
case BHY2_META_EVENT_SENSOR_ERROR:
printf("%s Sensor id %u reported error 0x%02X\r\n", event_text, byte1, byte2);
break;
case BHY2_META_EVENT_FIFO_OVERFLOW:
printf("%s FIFO overflow\r\n", event_text);
break;
case BHY2_META_EVENT_DYNAMIC_RANGE_CHANGED:
printf("%s Dynamic range changed for sensor id %u\r\n", event_text, byte1);
break;
case BHY2_META_EVENT_FIFO_WATERMARK:
printf("%s FIFO watermark reached\r\n", event_text);
break;
case BHY2_META_EVENT_INITIALIZED:
printf("%s Firmware initialized. Firmware version %u\r\n", event_text, ((uint16_t)byte2 << 8) | byte1);
break;
case BHY2_META_TRANSFER_CAUSE:
printf("%s Transfer cause for sensor id %u\r\n", event_text, byte1);
break;
case BHY2_META_EVENT_SENSOR_FRAMEWORK:
printf("%s Sensor framework event for sensor id %u\r\n", event_text, byte1);
break;
case BHY2_META_EVENT_RESET:
printf("%s Reset event\r\n", event_text);
break;
case BHY2_META_EVENT_SPACER:
break;
default:
printf("%s Unknown meta event with id: %u\r\n", event_text, meta_event_type);
break;
}
}
int8_t SensorFusionCluster_Init(void)
{
int8_t rslt;
uint8_t product_id = 0;
uint8_t boot_status = 0;
uint8_t hintr_ctrl = BHY2_ICTL_DISABLE_STATUS_FIFO | BHY2_ICTL_DISABLE_DEBUG;
bhi260_reset();
rslt = bhy2_init(BHY2_SPI_INTERFACE, bhy2_spi_read, bhy2_spi_write,
bhy2_delay_us, WORK_BUFFER_SIZE, NULL, &bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_soft_reset(&bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_get_product_id(&product_id, &bhi260);
if (rslt != BHY2_OK || product_id != BHY2_PRODUCT_ID) return -1;
rslt = bhy2_set_host_interrupt_ctrl(hintr_ctrl, &bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_get_boot_status(&boot_status, &bhi260);
if ((rslt != BHY2_OK) || !(boot_status & BHY2_BST_HOST_INTERFACE_READY)) return -2;
rslt = bhy2_upload_firmware_to_ram(bhy2_firmware_image, sizeof(bhy2_firmware_image), &bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_boot_from_ram(&bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_register_fifo_parse_callback(BHY2_SYS_ID_META_EVENT, parse_meta_event, NULL, &bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_register_fifo_parse_callback(BHY2_SYS_ID_META_EVENT_WU, parse_meta_event, NULL, &bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_register_fifo_parse_callback(QUAT_SENSOR_ID, parse_quaternion, NULL, &bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_update_virtual_sensor_list(&bhi260);
if (rslt != BHY2_OK) return rslt;
rslt = bhy2_set_virt_sensor_cfg(QUAT_SENSOR_ID, SAMPLE_RATE_HZ, REPORT_LATENCY, &bhi260);
if (rslt != BHY2_OK) return rslt;
return BHY2_OK;
}
void SensorFusionCluster_Process(void)
{
int8_t rslt = bhy2_get_and_process_fifo(work_buffer, WORK_BUFFER_SIZE, &bhi260);
if (rslt != BHY2_OK)
{
printf("FIFO process error: %d\r\n", rslt);
}
}
And on another file i execute everything as follow (just for test purposes) :
res = SensorFusionCluster_Init();
if(res != 0){
RGB_LED_SetPredefinedColor(RED); // Sets the RGBW Led to OK
return STATUS_ERROR;
}
while (1)
{
SensorFusionCluster_Process();
HAL_Delay(300); // Polling every 100 ms
} The initialization is executed successfully without errors, but when I go through the SensorFusionCluster_Process() function, I get strange data :
At the beginning we get a BHY2_META_EVENT_SPACER (i assume it's ok for the first loop):
After that i get a BHY2_META_EVENT_INITIALIZED (also good i assume) :
Those two events occurs two times.
After that i get a BHY2_META_EVENT_FIFO_OVERFLOW (I'm not sure this is good) :
And once i get this event, the parse_quaternion() function is called, giving me those results :
Those values does not change that much, and i don't understand why. Here's the result after some loops :
Lastly, here's my common.c file where i have the spi read/write and useful functions :
#include "drivers_h/bhi260_sensor_cluster/common.h"
#include "main.h"
#include "drivers_h/bhi260_sensor_cluster/bhy2.h"
#define BHY2_SPI_TIMEOUT 1000
#define BHY2_SPI_MAX_LENGTH 256
static void delay_us(uint32_t us)
{
uint32_t delay = (SystemCoreClock / 1000000U / 2U) * us;
while (delay--) __NOP();
}
void bhy2_delay_us(uint32_t us, void *private_data)
{
(void)private_data;
delay_us(us);
}
int8_t bhy2_spi_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr)
{
(void)intf_ptr;
if (reg_data == NULL || length == 0 || length > BHY2_SPI_MAX_LENGTH)
return BHY2_E_IO;
uint8_t tx_buf[1] = { reg_addr | 0x80 }; // Set MSB for read
static uint8_t rx_buf[BHY2_SPI_MAX_LENGTH + 1]; // Static prevents stack overflow
HAL_GPIO_WritePin(BHI260_CS_PORT, BHI260_CS_PIN, GPIO_PIN_RESET);
HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(&hspi4, tx_buf, rx_buf, length + 1, BHY2_SPI_TIMEOUT);
HAL_GPIO_WritePin(BHI260_CS_PORT, BHI260_CS_PIN, GPIO_PIN_SET);
if (status == HAL_OK)
{
memcpy(reg_data, &rx_buf[1], length); // Skip dummy byte
return BHY2_OK;
}
return BHY2_E_IO;
}
int8_t bhy2_spi_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr)
{
(void)intf_ptr;
if (reg_data == NULL || length == 0 || length > BHY2_SPI_MAX_LENGTH)
return BHY2_E_IO;
static uint8_t tx_buf[BHY2_SPI_MAX_LENGTH + 1];
tx_buf[0] = reg_addr & 0x7F;
memcpy(&tx_buf[1], reg_data, length);
HAL_GPIO_WritePin(BHI260_CS_PORT, BHI260_CS_PIN, GPIO_PIN_RESET);
HAL_StatusTypeDef status = HAL_SPI_Transmit(&hspi4, tx_buf, length + 1, BHY2_SPI_TIMEOUT);
HAL_GPIO_WritePin(BHI260_CS_PORT, BHI260_CS_PIN, GPIO_PIN_SET);
return (status == HAL_OK) ? BHY2_OK : BHY2_E_IO;
}
void bhi260_reset(void)
{
HAL_GPIO_WritePin(BHI260_RST_PORT, BHI260_RST_PIN, GPIO_PIN_RESET);
HAL_Delay(50);
HAL_GPIO_WritePin(BHI260_RST_PORT, BHI260_RST_PIN, GPIO_PIN_SET);
HAL_Delay(100); // Allow time to boot
}
bool get_interrupt_status(void)
{
return (HAL_GPIO_ReadPin(BHI260_INT_PORT, BHI260_INT_PIN) == GPIO_PIN_SET);
}I hope I showed the complete picture about my issue.
Can someone help me debug this? It's really important, thanks in advance!