BMX160 and BSXLite integration questions

[doctorshtein]

Hi,

I'm working on a project where I need quite precise (0.1 of a degree) measurements of gravity vector and north direction.

For this project I'm using BMX160 (board is from DFRobot) + Teensy 4.1 for hardware, BMXLite & Arduino for software.

I've read pretty much all topics about BMX160 & BSXLite on this forum and crafted some code according to all the recommendations.

My code seems to work more or less fine, but measurements are not always precise, and I got several questions, which I hope you can help me find answers to.

1. MSXLite for BMX only supports ODR of 100Hz for Accelerometer & Gyro, and 25Hz for Magnetometer. I've configured ODRs to  BMI160_ACCEL_ODR_100HZ, BMI160_GYRO_ODR_100HZ, and BMI160_AUX_ODR_25HZ. My question is: Can i bring ODR's higher so that I can use more advanced filters (e.g. BMI160_ACCEL_BW_OSR_AVG4). Will it improve precision? Will BSXLite be able to handle it?
2. aux_rd_burst_len is set to 2 in the examples, but some people earlier suggested using 1 instead. Is there a difference? DOes one work better than another?
3. BSXLite integration examples all set magnetometer to preset_mode = BMM150_PRESETMODE_REGULAR. I observe quite an amount of noise in such a setting (up to several degrees). I wonder if it has to be REGULAR, or can it be BMM150_PRESETMODE_HIGHACCURACY? Will BSXLite be able to handle it?
4. As part of my code i perfom FOC after initialization and before measuring. How does this work with BSXLite? Is it needed? Is it helping or making things worse?
5. Want to confirm that BSX_WORKINGMODE_NDOF_GEORV_FMC_OFF is the only mode supported in BSXLite?
6. BSXLite itnegration examples do call bsx_get_hwdependency and results are discarded. What is the benefit of doing so? Is it necessary?
7. Some examples of BSXLite integrations do call bsx_set_accdatarate, bsx_set_magdatarate, bsx_set_gyrodatarate. Some people use 100,25,100 as parameters, and others use 9,6,9. Following the code comments in bxslite header file, it should be 9,6,9. I didn't observe visible differences. What are the correct values? and Should those methods be called at all?
8. When passing data to BSXLite's dostep function, some people remap the axis. For example, i've seen people swapping x and y axis. and also negating x and z axis for the Magnetometer. Aren't the sensors (magnetometer and accel / gyro) aligned inside BMX160? Why do people remap axis? What does it help with? Why do some revert sign of magnetomere axis?
9. BSXLite accepts a timestamp with the data. I've seen people sending sensortime * 39 and millis()*1000. Which one is better? Is one of them incorrect?
10. Since magnetometer is at 25Hz, some examples do resend the same old values (with an old timestamp) for magnetometer 4 times. Other example re-read magnetometer data at 100Hz and send it with new timestamps at 100Hz. Which one is correct? Which one is better?
11. Since BMM150 returns corrected data in uT and not in LBS, i'm assuming results from bmm150_aux_mag_data have to be multiplied by 16 before sending to BSXLite. Is that correct?
12. Does BSXLite perform soft & hard iron calibration of the magnetometer somehow? Or Should I do that before sending data to BSXLite. Any advice for how to best approach it?
13. Accelerometer takes a long time and a very unpredictable set of movements to calibrate after start (to reach bsx_get_acccalibaccuracy == 3). Is that expected?
14. Acceleromoter does lose calibration (drops to bsx_get_acccalibaccuracy  == 0) quite often. Any ideas what can be causing it? or how can I prevent it?
15. When I read BSXLite documentation, i thought pitch should be a rotation around Y and roll - around X. It's vice versa in BSX's output. Am i doing something wrong or is it expected?
16. bsx_get_geoheadingaccuracy_rad is very high (30 degrees after conversion from rad) even when i reach 3 on all sensor's calibration accuracy. Why is it so high? How can I improve it?
17. Heading values drift a lot, especially after movign a sensor around. Heading == 0 doesn't point North (accounting for magnetic declination) and is usually off by +- 5 degrees. What can I do with it?
18. Raw and compensated values for magnetometer are very unstable and noisy. That together with drifting heading values doesn't let me identify north direction precisely. Any advice how can i improve it?

Here's the sample output of my code. Code is also below. Thanks in advance for all the help!

AMG Accuracy: 3, 3, 3
Euler: H=2.0505, Y=2.0505, P=-6.7879, R=-0.9730,  Acc=3 Deg=18.0013

A raw: -0.2245, 1.2192, 9.7588
G raw: 0.0025, 0.0006, -0.0237
M raw: -11.0000, 13.0000, -65.0000
M cor: 1.0000, 14.8000, -58.0000
North offset raw -40.2364,  corr 3.8655




AMG Accuracy: 3, 3, 3
Euler: H=2.0505, Y=2.0505, P=-6.7879, R=-0.9730,  Acc=3 Deg=18.0013

A raw: -0.2245, 1.2192, 9.7588
G raw: 0.0025, 0.0006, -0.0237
M raw: -12.0000, 13.0000, -64.0000
M cor: 0.0000, 14.8000, -57.0000
North offset raw -42.7094,  corr 0.0000

#include <Arduino.h>
#include <Wire.h>

#include <bmi160.h>
#include <bmm150.h>

#include <BsxFusionLibrary.h>
#include <BsxLibraryCalibConstants.h>
#include <BsxLibraryConstants.h>
#include <BsxLibraryDatatypes.h>
#include <BsxLibraryErrorConstants.h>

BSX_U8 kAccelSpec[] = {37,0,3,1,0,9,12,150,0,16,60,0,1,0,1,0,176,4,82,3,0,0,64,65,1,1,1,1,2,2,2,3,3,1,1,180,115};
BSX_U8 kMagnSpec[] = {39,0,2,1,20,5,20,5,196,9,6,9,112,23,0,0,128,61,205,204,76,63,0,0,224,64,1,1,1,1,1,1,1,1,1,1,1,134,84};
BSX_U8 kGyroSpec[] = {14,0,1,1,3,9,12,136,19,16,1,1,129,46};
BSX_U8 kUsecaseSpec[] = {116,6,1,1,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,51,51,179,62,205,204,12,63,205,204,12,63,51,51,51,63,51,51,51,63,205,204,76,63,1,0,9,4,2,23,183,209,56,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,23,183,209,56,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,23,183,209,56,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,189,55,134,53,189,55,134,53,189,55,134,53,0,0,0,0,0,0,16,66,232,3,5,0,45,0,132,3,176,4,150,0,8,150,0,13,1,1,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,51,51,179,62,205,204,12,63,205,204,12,63,51,51,51,63,51,51,51,63,205,204,76,62,1,6,4,1,0,5,0,65,1,64,1,36,0,120,0,4,1,20,20,2,2,0,4,0,0,128,63,205,204,204,61,154,153,153,63,205,204,204,62,205,204,204,61,1,0,20,0,16,4,120,0,8,0,0,5,154,153,25,63,154,153,25,63,80,0,9,0,30,0,232,3,80,0,65,0,4,0,4,0,0,128,62,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,62,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,62,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,181,254,22,55,181,254,22,55,181,254,22,55,139,222,169,56,0,0,224,64,13,1,1,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,205,204,204,61,1,9,9,3,19,50,163,4,205,12,100,40,4,13,0,1,154,153,153,62,154,153,153,62,205,204,204,62,154,153,25,63,154,153,153,62,0,0,128,62,154,153,153,62,236,81,184,62,205,204,76,63,205,204,76,63,205,204,76,63,205,204,76,63,205,204,76,62,205,204,76,62,205,204,76,62,205,204,76,62,0,194,184,178,62,53,250,142,60,10,0,10,0,0,2,0,10,0,80,119,86,61,13,0,0,128,62,143,194,245,60,10,215,163,60,100,128,52,45,70,1,10,0,80,0,0,0,192,63,0,0,0,64,9,2,0,0,200,65,0,0,128,66,0,0,128,65,0,0,192,63,205,204,76,61,194,184,178,61,50,37,59,24,71,0,0,160,64,154,153,25,63,80,119,86,61,0,1,205,204,76,63,0,0,96,64,0,0,32,64,205,204,204,61,4,143,194,245,60,2,1,2,3,4,1,10,176,4,88,2,10,215,35,60,10,0,10,0,0,0,250,67,0,0,122,68,0,0,160,63,0,0,72,66,0,0,128,63,0,0,128,62,205,204,204,61,0,0,32,66,0,0,128,62,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,62,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,62,0,36,116,73,0,0,0,0,0,0,0,0,0,0,0,0,0,36,116,73,0,0,0,0,0,0,0,0,0,0,0,0,0,36,116,73,0,0,192,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,192,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,192,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,64,10,215,35,60,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,10,215,35,60,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,10,215,35,60,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,23,183,209,56,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,23,183,209,56,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,23,183,209,56,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,128,63,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,172,197,39,55,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,172,197,39,55,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,172,197,39,55,0,36,116,73,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,36,116,73,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,36,116,73,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,4,3,0,0,0,0,0,0,10,3,4,25,64,18,24,0,64,114,8,0,13,226,109};

bmi160_dev g_main_sensor;
bmm150_dev g_aux_sensor;
bmm150_settings g_aux_settings; 
uint8_t g_bmm150_dev_address = BMM150_DEFAULT_I2C_ADDRESS;

void uint32_delay(uint32_t ms)
{
  delay(ms);
}

int8_t i2c_read(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
  Wire.beginTransmission(dev_addr);
  Wire.write(reg_addr);
  uint8_t result = Wire.endTransmission();
  if (result != 0)
  {
    return result;
  }
  Wire.requestFrom(dev_addr, (uint8_t)len);
  for (uint16_t i = 0; i < len; i++)
  {
    data[i] = Wire.read();
  }
  return Wire.endTransmission();
}

int8_t i2c_write(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
  Wire.beginTransmission(dev_addr);
  Wire.write(reg_addr);
  for (uint16_t i = 0; i < len; i++)
  {
    Wire.write(data[i]);
  }
  return Wire.endTransmission();
}

int8_t bmm150_user_i2c_reg_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr)
{
  return bmi160_aux_write(reg_addr, const_cast<uint8_t*>(reg_data), length, &g_main_sensor);
}

int8_t bmm150_user_i2c_reg_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr)
{
  return bmi160_aux_read(reg_addr, reg_data, length, &g_main_sensor);
}

void bmm150_user_delay_us(uint32_t period_us, void *intf_ptr)
{
    delayMicroseconds(period_us);
}

int8_t bmx160_init()
{
  g_main_sensor.id = BMI160_I2C_ADDR;
  g_main_sensor.interface = BMI160_I2C_INTF;
  g_main_sensor.read = i2c_read;
  g_main_sensor.write = i2c_write;
  g_main_sensor.delay_ms = uint32_delay;

  int8_t init_result = bmi160_init(&g_main_sensor);
  if (init_result != BMI160_OK)
  {
    Serial.print("bmi160_init: ");
    Serial.println(init_result);
    return init_result;
  }

  g_main_sensor.accel_cfg.odr = BMI160_ACCEL_ODR_100HZ; // TODO: Can this be higher with a different g_main_sensor.accel_cfg.bw value?
  g_main_sensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G;
  g_main_sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4;
  g_main_sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE;

  g_main_sensor.gyro_cfg.odr = BMI160_GYRO_ODR_100HZ; // TODO: Can this be higher with a different g_main_sensor.accel_cfg.bw value?
  g_main_sensor.gyro_cfg.range = BMI160_GYRO_RANGE_500_DPS;
  g_main_sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE;
  g_main_sensor.gyro_cfg.power = BMI160_GYRO_NORMAL_MODE;

  init_result = bmi160_set_sens_conf(&g_main_sensor);
  if (init_result != BMI160_OK)
  {
    Serial.print("bmi160_set_sens_conf: ");
    Serial.println(init_result);
    return false;
  }

  g_main_sensor.aux_cfg.aux_sensor_enable = 1;                     // auxiliary sensor enable
  g_main_sensor.aux_cfg.aux_i2c_addr = BMI160_AUX_BMM150_I2C_ADDR; // auxiliary sensor address
  g_main_sensor.aux_cfg.manual_enable = 1;                         // setup mode enable
  g_main_sensor.aux_cfg.aux_rd_burst_len = 2;                      // burst read of 2 byte
  
  //TODO: Chech if aux_rd_burst_len actually works. It used to fail and someone recommended to set it to 1

  g_aux_sensor.read = bmm150_user_i2c_reg_read;
  g_aux_sensor.write = bmm150_user_i2c_reg_write;
  g_aux_sensor.intf_ptr = &g_bmm150_dev_address;
  g_aux_sensor.delay_us = bmm150_user_delay_us;
  g_aux_sensor.intf = BMM150_I2C_INTF;

  init_result = bmi160_aux_init(&g_main_sensor);
  if (init_result != BMI160_OK)
  {
    Serial.print("bmi160_aux_init: ");
    Serial.println(init_result);
    return init_result;
  }

  init_result = bmm150_init(&g_aux_sensor);
  if (init_result != BMM150_OK)
  {
    Serial.print("bmm150_init: ");
    Serial.println(init_result);
    return init_result;
  }

  g_aux_settings.preset_mode = BMM150_PRESETMODE_REGULAR;  //TODO: Can this be high accuracy ?
  init_result = bmm150_set_presetmode(&g_aux_settings, &g_aux_sensor);
  if (init_result != BMM150_OK)
  {
    Serial.print("bmm150_set_presetmode: ");
    Serial.println(init_result);
    return init_result;
  }

  g_aux_settings.pwr_mode = BMM150_POWERMODE_FORCED;
  init_result = bmm150_set_op_mode(&g_aux_settings, &g_aux_sensor);
  if (init_result != BMM150_OK)
  {
    Serial.print("bmm150_set_op_mode: ");
    Serial.println(init_result);
    return init_result;
  }

  uint8_t aux_addr = 0x42;
  g_main_sensor.aux_cfg.aux_odr = BMI160_AUX_ODR_25HZ;
  init_result = bmi160_set_aux_auto_mode(&aux_addr, &g_main_sensor);
  if (init_result != BMI160_OK)
  {
    Serial.print("bmi160_set_aux_auto_mode: ");
    Serial.println(init_result);
    return init_result;
  }

  
  // TODO: How does FOC works together with BSX Lite? Is it needed? Is it desired?
  bmi160_foc_conf foc_conf;
  bmi160_offsets internal_offsets;
  foc_conf.acc_off_en = BMI160_ENABLE;
  foc_conf.gyro_off_en = BMI160_ENABLE;
  foc_conf.foc_acc_x = BMI160_FOC_ACCEL_0G;
  foc_conf.foc_acc_y = BMI160_FOC_ACCEL_0G;
  foc_conf.foc_acc_z = BMI160_FOC_ACCEL_POSITIVE_G;
  foc_conf.foc_gyr_en = BMI160_ENABLE;

  init_result = bmi160_start_foc(&foc_conf, &internal_offsets, &g_main_sensor);
  if (init_result != BMI160_OK)
  {
    Serial.print("bmi160_start_foc: ");
    Serial.println(init_result);
    return init_result;
  }

  delay(250);
  return BMI160_OK;
}

int8_t bmx160_read_data(bmi160_sensor_data* accel_data, bmi160_sensor_data* gyro_data, bmm150_mag_data* mag_data)
{
  uint8_t data[23] = {0};
  int8_t result = bmi160_get_regs(BMI160_AUX_DATA_ADDR, data, 23, &g_main_sensor);
  if (result != BMI160_OK)
  {
    Serial.print("bmi160_get_regs: ");
    Serial.println(result);
    return result;
  }

  uint8_t time_0 = data[20];
  uint16_t time_1 = ((uint16_t)data[21]) << 8;
  uint32_t time_2 = ((uint32_t)data[22]) << 16;
  uint32_t sensortime = (uint32_t)(time_2 | time_1 | time_0);

  result = bmm150_aux_mag_data(data, mag_data, &g_aux_sensor);
  if (result != BMM150_OK) {
    Serial.print("bmm150_aux_mag_data: ");
    Serial.println(result);
    return result;
  }

  gyro_data->x = (int16_t)((data[9] << 😎 | data[8]);
  gyro_data->y = (int16_t)((data[11] << 😎 | data[10]);
  gyro_data->z = (int16_t)((data[13] << 😎 | data[12]);
  gyro_data->sensortime = sensortime;

  accel_data->x = (int16_t)((data[15] << 😎 | data[14]);
  accel_data->y = (int16_t)((data[17] << 😎 | data[16]);
  accel_data->z = (int16_t)((data[19] << 😎 | data[18]);
  accel_data->sensortime = sensortime;

  return BMI160_OK;
}

int8_t bsxlite_init() {
  initParam_t s_input;
  s_input.accelspec = kAccelSpec;
  s_input.magspec = kMagnSpec;
  s_input.gyrospec = kGyroSpec;
  s_input.usecase = kUsecaseSpec;

  BSX_S8 init_status = bsx_init(&s_input);
  if (init_status != BSX_STATE_OK)
  {
    Serial.print("bsx_init: ");
    Serial.println(init_status);
    return init_status;
  }

  ts_workingModes s_workingmodes;
  s_workingmodes.opMode = BSX_WORKINGMODE_NDOF_GEORV_FMC_OFF; // TODO: Is taht really the only one supprted by the bsx lite?
  init_status = bsx_set_workingmode(&s_workingmodes);
  if (init_status != BSX_STATE_OK)
  {
    Serial.print("bsx_set_workingmode: ");
    Serial.println(init_status);
    return init_status;
  }

  ts_HWsensorSwitchList s_hwSensorSwitchList;
  init_status = bsx_get_hwdependency(s_workingmodes, &s_hwSensorSwitchList); // TODO: Why do we need to call this function? DOes it have interna side-effects?
  if (init_status != BSX_STATE_OK)
  {
    Serial.print("bsx_get_hwdependency: ");
    Serial.println(init_status);
    return init_status;
  }

  // TODO: There are contradicting examples, where people set these to 100, 20, 100 and not to what's in the comments.
  init_status = bsx_set_accdatarate(9); //100
  if (init_status != BSX_STATE_OK)
  {
    Serial.print("bsx_set_accdatarate: ");
    Serial.println(init_status);
    return init_status;
  }
  init_status = bsx_set_magdatarate(6); //25
  if (init_status != BSX_STATE_OK)
  {
    Serial.print("bsx_set_magdatarate: ");
    Serial.println(init_status);
    return init_status;
  }
  init_status = bsx_set_gyrodatarate(9); // 100
  if (init_status != BSX_STATE_OK)
  {
    Serial.print("bsx_set_gyrodatarate: ");
    Serial.println(init_status);
    return init_status;
  }

  return BSX_STATE_OK;
}

void setup()
{
  Serial.begin(115200);
  Wire.begin();
  Wire.setClock(400000);
  delay(100);

  if (bmx160_init() != BMI160_OK) {
    while(1);
  }
  delay(100);

  if (bsxlite_init() != BSX_STATE_OK) {
    while(1);
  }
  delay(100);
}


libraryinput_t libraryInput_ts = {0};
uint32_t timestamp = 0;
int cycle_id = 0;

void loop() {
  if (timestamp + 10 > millis()) {
    return;
  }
  timestamp = millis();

  bmi160_sensor_data accel_data, gyro_data;
  bmm150_mag_data mag_data;
  if (bmx160_read_data(&accel_data, &gyro_data, &mag_data) != BMI160_OK) {
    while(1);
  }

  // TODO: Some people remap axis (swap x with y; negate mag axis). What does that do? Should it be done?
  libraryInput_ts.acc.data.x = accel_data.x;
  libraryInput_ts.acc.data.y = accel_data.y;
  libraryInput_ts.acc.data.z = accel_data.z;
  libraryInput_ts.acc.time_stamp =  accel_data.sensortime * 39; //timestamp * 1000;
  // TODO: Is sensortime * 39 better than timestamp * 1000?

  libraryInput_ts.gyro.data.x = gyro_data.x;
  libraryInput_ts.gyro.data.y = gyro_data.y;
  libraryInput_ts.gyro.data.z = gyro_data.z;
  libraryInput_ts.gyro.time_stamp =  accel_data.sensortime * 39; //timestamp * 1000;

  // TODO: Should I update data and timestamp for mag data only every 4 cycles, or on every cycle anyway? BSX wants 25Hz
  if (cycle_id == 0) { 
    libraryInput_ts.mag.data.x = mag_data.x * 16;  // TODO: SHould I perform this *16 correction? BMM150 returns data in uT and not in LBS
    libraryInput_ts.mag.data.y = mag_data.y * 16;
    libraryInput_ts.mag.data.z = mag_data.z * 16;
    libraryInput_ts.mag.time_stamp =  accel_data.sensortime * 39; //timestamp * 1000;
  }
  cycle_id = (cycle_id + 1) % 4;
    
  BSX_S8 step_result = bsx_dostep(&libraryInput_ts);
  if (step_result != BSX_STATE_OK) {
    Serial.print("bsx_dostep: ");
    Serial.println(step_result);
    while(1);
  }

  if (cycle_id == 1) {
    BSX_U8 acc_status = 0, gyro_status = 0, mag_status = 0;
    bsx_get_acccalibaccuracy(&acc_status); // TODO: accelerometer takes forever to calibrate and looses calibration often. WDYD?
    bsx_get_gyrocalibaccuracy(&gyro_status);
    bsx_get_magcalibaccuracy(&mag_status); // TODO: Is this a soft or hard iron calibration?
    
    ts_dataxyzf32 accRawData, gyroRawData, magRawData, magCompData;
    bsx_get_accrawdata(&accRawData);
    bsx_get_gyrorawdata_rps(&gyroRawData);
    bsx_get_magrawdata(&magRawData);
    bsx_get_magcordata(&magCompData); // TODO: How does this work with magnetometer soft & hard iron calibration?
    
    ts_dataeulerf32 orientEuler_rad;
    bsx_get_orientdata_euler_rad(&orientEuler_rad);
    // TODO: Why is roll around y and pitch is around x axis? can i swap them? what happens to foc and calibration?

    BSX_F32 accuracy_rad = 0;
    bsx_get_geoheadingaccuracy_rad(&accuracy_rad);
    // TODO: This is very high (30 deg). Why?

    BSX_U8 orient_status = 5;
    bsx_get_orient_datastatus(&orient_status);

    double north_delta_raw = atan2(magRawData.x, magRawData.y) * 180.0 / PI;
    double north_delta_corr = atan2(magCompData.x, magCompData.y) * 180.0 / PI;
    // TODO: This is very unstable? Is there a way to make this more stable?
 
    Serial.write(12);               
    Serial.printf("AMG Accuracy: %d, %d, %d  ", acc_status, mag_status, gyro_status);
    Serial.println();
    Serial.printf("Euler: H=%.4f, Y=%.4f, P=%.4f, R=%.4f,  Acc=%d Deg=%.4f\n", orientEuler_rad.h*57.3, orientEuler_rad.y*57.3, orientEuler_rad.p*57.3, orientEuler_rad.r*57.3, orient_status, accuracy_rad*57.3);
    Serial.println();
    Serial.printf("A raw: %.4f, %.4f, %.4f  ", accRawData.x, accRawData.y, accRawData.z);
    Serial.println();
    Serial.printf("G raw: %.4f, %.4f, %.4f  ", gyroRawData.x, gyroRawData.y, gyroRawData.z);
    Serial.println();
    Serial.printf("M raw: %.4f, %.4f, %.4f  ", magRawData.x, magRawData.y, magRawData.z);
    Serial.println();
    Serial.printf("M cor: %.4f, %.4f, %.4f  ", magCompData.x, magCompData.y, magCompData.z);
    Serial.println();
    Serial.printf("North offset raw %.4f,  corr %.4f", north_delta_raw, north_delta_corr);
    Serial.println();
    Serial.println();
  }
}

[Vincent · Accepted solution]

Please let me try to answer your question one by one: 

1.  BSX lite has fixed ODR.  which you already mentioned accl / gyro use 100Hz and magnetic data use 25Hz.  and output data rate is fixed as 100Hz. 

2.  This means BSX lite will not able to work with any other different ODR for input and output.   You should always keep same input data rate and output data rate.  Otherwise,  you will get error code

3.  BMI160_ACCEL_BW_OSR_AVG4 is the filter setting.  not ODR related.  so you can keep ODR as 100Hz but use low noise filter.  since the ODR is still 100Hz,  the BSX running have no any impact.  but increased ODR means increased signal latency.  So you heading value output from BSX might be slow than you motion.  if you are not in real time application,  then it is fine. 

4. aux_burst_read_length is used to ask BMI160 to read magnetic sensor data from BMM150 on its secondary interface.  On BMX160,  the internal structure is BMM150 connect to BMI150 secondary interface.  Here I highly recommend you to follow our API to set length as 2 not using 1 instead to make sure you don't loose any data

5. Magnetic sensor ODR is defined by the preset mode.  Regular mode can achieve up to 100Hz ODR with big noise.  BMM150_PRESETMODE_HIGHACCURACY can only achieve 20Hz which is lower than the BSX lite lib input data rate requirement.  So we can't use it.  But you can try with enhance regular preset which can achieve up to 60Hz.   

6. BSX lib have offset compensation module running inside.  the FOC will not impact the performance of BSX lite,  but just speed up the calibration of BSX

7.  Yes, BSX lite only support BSX_WORKINGMODE_NDOF_GEORV_FMC_OFF mode

8.  The remapping is used to align the sensor axis to your system axis.  So sensor coordinate system can be unified to system coordinate.  Then the BSX output can be directly used for system orientation.  how to do remapping or whether it is necessary to do remapping is rely on the HW design how you align sensor axis to your system coordinate. 

9.  bsx_set_accdatarate is recommend to be called and use the number 9.6.9 

10. Since BSX lite only have one working mode,  bsx_get_hwdependency is not mandatory to be called.  you can also ignore it

11. BSX lib will checking the time stamp for each input data and make sure the jitter is below 15%.  So if you time stamp is accurate enough,  you can use any time source to do so.  The sensor time is quite accurate source,  so a lot of people directly use them. 

12.  for Magnetic data,  just make sure you send to lib with 25Hz.   You can repeat Mag data to align with accel and gyro as 100Hz,  or just keep as input for 25Hz.  there is no difference or impact on lib output 

13.  the magnetic sensor input unit for BSX is uT.   bmm150_aux_mag_data  API output is also uT,  just in float type or int16 type.  so no need to multiple by number 16. 

14. hard iron was process as offset of magnetic data,  so it is done inside BSX.  but soft iron is not able to processed by the BSX.  you need calculate the SIC matrix and input as a parameters to BSX. 

15.  Acceleration sensor calibration need to go through all 6 direction to get it calibrated.  you can refer to this video:

https://www.youtube.com/watch?v=Bw0WuAyGsnY  

16.  the acceleration calibration status lost is a bug of BSX lite version.   In normal BSX full version,  after acceleration sensor get calibrated,  the level should be kept. 

17.  Please do align sensor coordinate with your system coordianate first (here means remapping) first.  then check the result again.  

18.  if heading is accurate and drift,  then you need to consider the test environment have no big magnetic distortion and also you perform the soft iron matrix correctly. 

19. bsx_get_geoheadingaccuracy_rad is used for different working mode,  for the current working mode of BSX lite,  this function have no meaning. 

20.  if raw magnetic sensor data is changing quite a lot means your test environment have a lot of distortion source.  so the heading value drift is expected and BSX lib is not able to compensate for it.  

21.  according to your code,  overall is fine,  but i have one suggestion:  for getting the heading deviation,  why not directly compare the heading output from orientation data with ground truth or reference sensor?  the calculate with raw mag and compensated mag for heading deviation is not too much meaningful here.   

22.  For bsx_get_orientdata_euler_rad,   you just need to take h (heading), p (pitch), r (roll) and status.  rest are not really useful.  you can ignore it. 

[doctorshtein]

Hi Vincent,
thank you very much for your detailed reply! I really appreciate it! 

I'm marking your reply as an answer, yet I have a couple of minor follow up questions. I would really appreciate your help with those too.

1. Just of curiosity, how much of a delay should I expect from BMI160_ACCEL_BW_OSR_AVG4?
   
   
2. Regarding your answer #12
   

   ---

   @Vincent wrote:
   

   12.  for Magnetic data,  just make sure you send to lib with 25Hz.   You can repeat Mag data to align with accel and gyro as 100Hz,  or just keep as input for 25Hz.  there is no difference or impact on lib output 
   

   ---

   I'm not completely sure how can I send magnetometer data at 25Hz, while sending Accel and Gyro data at 100Hz. bsx_dostep method accepts one struct, which holds data for all 3 sensors. Should I zero-out magnetometer part explicitly or is there another way to let BSX know that magnetometer data is absent?
   
   
3. Regarding soft iron calibration. You suggest passing SIC matric as a parameter to BSX. Unfortunately, bsx_set_softiron_correctionmatrix is not exposed by the pre-compiled library, even though it is in the header. Is there a different way to pass the SIC matrix into BSX, or should I apply the matrix to magnetometer data before passing it to BSX?
   
   
4. What method can i use instead of bsx_get_geoheadingaccuracy_rad to get heading accuracy in rads? Is it bsx_get_headingaccuracy_rad?
   
   
5. For restoring calibration status on device initialization (so I don't have to re-calibrate on every device start-up), should I use 
   bsx_(get|set)_(acc|gyro|mag)calibprofile functions?
   
   
6. Following your suggestion to use BMM150_PRESETMODE_ENHANCED I could get bsx_get_headingaccuracy_rad down to 3 degrees (~0.0523 rad). It seems to be stably at that value. Is it really up to 3 degrees off or is it more precise in reality?
   
   
7. Regarding multiplying magnetometer data by 16 - if I stop doing taht, I can't get magnetometer to calibrate at all. CAlibration accuracy remains at 0. It appears multiplication by 16 is required to make it work?

[Vincent]

Let me try to answer your question quickly as following: 

1.  I will not just call it delay with  BMI160_ACCEL_BW_OSR_AVG4.  Samples will get with desired ODR you configured.  Here i call it group delay which is the duration from the real action and you read from sensor.  And this value affected by ODR also.  Assume you use 100Hz to work with BSX lite lib, the group delay is about 24 ms. 

2.  you input as acc,gyro,mag data structure.  for 100Hz,  you can input like ([acc, gyro], [acc,gyro],[acc, gyro],[acc,gyro,mag]) or repeat mag for all samples until you have new mag data.  please do NOT use 0 instead. 

3. BSX lite is not expose this function to customer this means with BSX lite,  you can't make Soft Iron Compensation. 

4. i suggest you skip the accuracy output from lib,  it has some bugs in BSX lite.   Instead of it,  compare the heading result with the reference sensor

5.  Yes,  please use bsx_(get|set)_(acc|gyro|mag)calibprofile functions. 

6.  BSX lite take uT as input.  Depending on which output function you used from our API and which unit you are input to lib.  If mag calibration is always 0 means the input mag data is wrong.  So you need to align with the input unit with the data you feed into lib. 

[doctorshtein]

Thanks again for a great response!