About 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.   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? 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? 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? 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? Want to confirm that BSX_WORKINGMODE_NDOF_GEORV_FMC_OFF is the only mode supported in BSXLite? BSXLite itnegration examples do call bsx_get_hwdependency and results are discarded. What is the benefit of doing so? Is it necessary? 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? 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? 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? 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? 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? 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? 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? 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? 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? 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? 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? 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(); } }     ... View more