BMI088 data not making sense

[nyameaama]

I have written this basic driver for BMI088 in which I am able to read registers from the sensor.

#include "bmi088.h"
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/i2c.h"

#define I2C_MASTER_SCL_IO 22 /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO 21 /*!< GPIO number used for I2C master data */
#define I2C_MASTER_NUM 0 /*!< I2C master i2c port number, the number of i2c peripheral interfaces available will depend on the chip */
#define I2C_MASTER_FREQ_HZ 400000 /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE 0 /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS 1000

#define BM1088_ACCEL_ADDRESS 0x18 /*!< Slave address of the BM1088_acceleromter sensor SD01 pull to GND */
#define BM1088_GYRO_ADDRESS 0x68 /*!< Slave address of the BM1088 gyroscope sensor SD02 pull to GND*/
int16_t ret = 0;

/*! Earth's gravity in m/s^2 */
#define GRAVITY_EARTH (9.80665f)

static const char *TAG = "BM1088 Module";

/**
* @brief Memory locations to store BM1088 accel and gyro Sensors data
*/
uint8_t accel_x[2] = {0};
uint8_t accel_y[2] = {0};
uint8_t accel_z[2] = {0};

uint8_t gyro_x[2] = {0};
uint8_t gyro_y[2] = {0};
uint8_t gyro_z[2] = {0};

/**
* @brief Convertion of BM1088 accel and gyro Sensors data into signed integer
*/
float accel_x_int16 = 0;
float accel_y_int16 = 0;
float accel_z_int16 = 0;


float gyro_x_int16 = 0;
float gyro_y_int16 = 0;
float gyro_z_int16 = 0;


/**
* @brief Convertion of BM1088 accel and gyro Sensors signed integer into acceleration
*/
float accel_x_in_mg = 0;
float accel_y_in_mg = 0;
float accel_z_in_mg = 0;

/**
* @brief calculate BM1088 data in signed form from LSB into Degree per second
*/
float gyro_x_in_degree = 0;
float gyro_y_in_degree = 0;
float gyro_z_in_degree = 0;

/**
* @brief Initialize angles
*/
double yaw = 0.0;
double pitch = 0.0;
double roll = 0.0;

/**
* @brief Initialize previous angles
*/
double prev_yaw = 0.0;
double prev_pitch = 0.0;
double prev_roll = 0.0;

/**
* @brief Initialize the previous timestamp
*/
int64_t previous_timestamp = 0;

/* Initialize other variables */
double pitch_acc = 0.0;
double roll_acc = 0.0;
double gyro_x_rad = 0.0;
double gyro_y_rad = 0.0;
double gyro_z_rad = 0.0;

/**
* @brief BM1088 IMU Register Addresses
*/
typedef enum
{
ACC_CHIP_ID = 0X00,
ACC_ERR_REG = 0X02,
ACC_STATUS = 0X03,
ACC_X_LSB = 0X12,
ACC_X_MSB = 0X13,
ACC_Y_LSB = 0X14,
ACC_Y_MSB = 0X15,
ACC_Z_LSB = 0X16,
ACC_Z_MSB = 0X17,
ACC_CONF = 0X40,
ACC_RANGE = 0X41,
ACC_SELF_TEST = 0X6D,
ACC_PWR_CONF = 0X7C,
ACC_PWR_CTRL = 0X7D,
ACC_SOFT_REST = 0X7E,
GYRO_CHIP_ID = 0X00,
GYRO_X_LSB = 0X02,
GYRO_X_MSB = 0X03,
GYRO_Y_LSB = 0X04,
GYRO_Y_MSB = 0X05,
GYRO_Z_LSB = 0X06,
GYRO_Z_MSB = 0X07,
GYRO_RANGE = 0X0F,
GYRO_BANDWIDTH = 0X10,
GYRO_LPM1 = 0X11,
GYRO_SOFT_REST = 0X14,
GYRO_SELF_TEST = 0X3C,

}mpu_register_address;

void BMI088_IMU::IMU_INIT(){
uint8_t data[2];
ESP_ERROR_CHECK(i2c_master_init());
ESP_LOGI(TAG, "I2C initialized successfully");

/* Read the BM1088 GYRO_CHIP_ID REGISTER, on power up the register should have the value 0x0F */
ESP_ERROR_CHECK(bm1088_gyro_read(GYRO_CHIP_ID, data, 1));
ESP_LOGI(TAG, "GYRO_CHIP_ID_REGISTER_VALUE = %X", data[0]);

/* Read the BM1088 ACC_PWR_CTRL REGISTER to check power on reset */
uint8_t check_por[2];
ESP_ERROR_CHECK(bm1088_accel_read(ACC_PWR_CTRL, check_por, 1));
vTaskDelay(1/ portTICK_PERIOD_MS);

/* Enable accel module by writing 0x04 to power control register */
uint8_t acc_pwr_ctr = 0x04;
acc_pwr_ctr |= check_por[0];

ESP_ERROR_CHECK(bm1088_accel_write_byte(ACC_PWR_CTRL, acc_pwr_ctr));
vTaskDelay(50/ portTICK_PERIOD_MS);
ESP_ERROR_CHECK(bm1088_accel_read(ACC_PWR_CTRL, check_por, 1));

if (check_por[0] == acc_pwr_ctr)
{
ESP_LOGI(TAG, "Accelerometer configured successfully");
}
else ESP_LOGI(TAG, "Accelerometer not configured ");
}

/**
* @brief Read a sequence of bytes from a BM1088 accel sensor registers
*/
esp_err_t BMI088_IMU::bm1088_accel_read(uint8_t reg_addr, uint8_t *data, size_t len){
for (size_t i = 0; i < len; i++) {
ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), BM1088_ACCEL_ADDRESS, (&reg_addr + i), 1,
(data + i), 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS );
}
return ret;
}

/**
* @brief Read a sequence of bytes from a BM1088 gyro sensor registers
*/
esp_err_t BMI088_IMU::bm1088_gyro_read(uint8_t reg_addr, uint8_t *data, size_t len){
for (size_t i = 0; i < len; i++) {
ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), BM1088_GYRO_ADDRESS, (&reg_addr + i), 1,
(data + i), 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS );
}
return ret;
}

/**
* @brief Write a byte to a BM1088 accel sensor register
*/
esp_err_t BMI088_IMU::bm1088_accel_write_byte(uint8_t reg_addr, uint8_t data){
uint8_t write_buf[2] = {reg_addr, data};

ret = i2c_master_write_to_device(i2c_port_t(I2C_MASTER_NUM), BM1088_ACCEL_ADDRESS, write_buf, sizeof(write_buf),
I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);

return ret;
}

/**
* @brief Write a byte to a BM1088 gyro sensor register
*/
esp_err_t BMI088_IMU::bm1088_gyro_write_byte(uint8_t reg_addr, uint8_t data){
uint8_t write_buf[2] = {reg_addr, data};

ret = i2c_master_write_to_device(i2c_port_t(I2C_MASTER_NUM), BM1088_GYRO_ADDRESS, write_buf, sizeof(write_buf),
I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);

return ret;
}

/**
* @brief i2c master initialization
*/
esp_err_t BMI088_IMU::i2c_master_init(void){
int i2c_master_port = I2C_MASTER_NUM;

i2c_config_t conf;
conf.mode = I2C_MODE_MASTER;
conf.sda_io_num = I2C_MASTER_SDA_IO;
conf.scl_io_num = I2C_MASTER_SCL_IO;
conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
conf.master.clk_speed = I2C_MASTER_FREQ_HZ;
conf.clk_flags = 0;


i2c_param_config(i2c_port_t(i2c_master_port), &conf);

return i2c_driver_install(i2c_port_t(i2c_master_port), conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

/*!
* @brief This function converts lsb to meter per second squared for 16 bit accelerometer at
* range 2G, 4G, 8G or 16G.
*/
float BMI088_IMU::lsb_to_mps2(int16_t val, int8_t g_range, uint8_t bit_width)
{
double power = 2;

float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));

return (GRAVITY_EARTH * val * g_range) / half_scale;
}

/*!
* @brief This function converts lsb to degree per second for 16 bit gyro at
* range 125, 250, 500, 1000 or 2000dps.
*/
float BMI088_IMU::lsb_to_dps(int16_t val, float dps, uint8_t bit_width)
{
double power = 2;

float half_scale = (float)((pow((double)power, (double)bit_width) / 2.0f));

return (dps / (half_scale)) * (val);
}

/*!
* @brief This function reads raw accel_x LSB data and converts to degree per second
*/
double BMI088_IMU::accel_read_rawX(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(bm1088_accel_read(ACC_X_LSB, accel_x, 2));
lsb = accel_x[0];
msb = accel_x[1];
msblsb = (msb << 8) | lsb;
float x_original_int = ((int16_t) msblsb); /* Data in X axis */
float x = lsb_to_mps2(x_original_int, 24, 16);
return x;
}

/*!
* @brief This function reads raw accel_y LSB data and converts to degree per second
*/
double BMI088_IMU::accel_read_rawY(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(bm1088_accel_read(ACC_Y_LSB, accel_y, 2));
lsb = accel_y[0];
msb = accel_y[1];
msblsb = (msb << 8) | lsb;
float y_original_int = ((int16_t) msblsb); /* Data in Y axis */
float y = lsb_to_mps2(y_original_int, 24, 16);
return y;
}

/*!
* @brief This function reads raw accel_z LSB data and converts to degree per second
*/
double BMI088_IMU::accel_read_rawZ(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(bm1088_accel_read(ACC_Z_LSB, accel_z, 2));
lsb = accel_z[0];
msb = accel_z[1];
msblsb = (msb << 8) | lsb;
float z_original_int = ((int16_t) msblsb); /* Data in Z axis */
float z = lsb_to_mps2(z_original_int, 24, 16);
return z;
}

/*!
* @brief This function reads raw gyro_x LSB data and converts to degree per second
*/
double BMI088_IMU::gyro_read_rawX(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(bm1088_gyro_read(GYRO_X_LSB, gyro_x, 2));
lsb = gyro_x[0];
msb = gyro_x[1];
msblsb = (msb << 8) | lsb;
float x_original_int = ((int16_t) msblsb); /* Data in X axis */
float x = lsb_to_dps(x_original_int, (float)250, 16);
return x;
}

/*!
* @brief This function reads raw gyro_y LSB data and converts to degree per second
*/
double BMI088_IMU::gyro_read_rawY(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(bm1088_gyro_read(GYRO_Y_LSB, gyro_y, 2));
lsb = gyro_y[0];
msb = gyro_y[1];
msblsb = (msb << 8) | lsb;
float y_original_int = ((int16_t) msblsb); /* Data in Y axis */
float y = lsb_to_dps(y_original_int, (float)250, 16);
return y;
}

/*!
* @brief This function reads raw gyro_z LSB data and converts to degree per second
*/
double BMI088_IMU::gyro_read_rawZ(){
uint8_t lsb, msb;
uint16_t msblsb;
ESP_ERROR_CHECK(bm1088_gyro_read(GYRO_Z_LSB, gyro_z, 2));
lsb = gyro_z[0];
msb = gyro_z[1];
msblsb = (msb << 8) | lsb;
float z_original_int = ((int16_t) msblsb); /* Data in Z axis */
float z = lsb_to_dps(z_original_int, (float)250, 16);
return z;
}

double BMI088_IMU::angle_read_pitch(){
double x_Buff = gyro_read_rawX();
double y_Buff = gyro_read_rawY();
double z_Buff = gyro_read_rawZ();
double pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
return pitch;
}

double BMI088_IMU::angle_read_roll(){
double y_Buff = gyro_read_rawY();
double z_Buff = gyro_read_rawZ();
double roll = atan2(y_Buff , z_Buff) * 57.3;
return roll;
}

double BMI088_IMU::angle_read_yaw(){
return 0;
}


Output:
I (332) app_start: Starting scheduler on CPU0
I (337) app_start: Starting scheduler on CPU1
I (337) main_task: Started on CPU0
I (347) main_task: Calling app_main()
I (347) BM1088 Module: I2C initialized successfully
I (347) BM1088 Module: GYRO_CHIP_ID_REGISTER_VALUE = F
I (407) BM1088 Module: Accelerometer configured successfully
I (407) BM1088 Module: accel_read_rawX = 1.824382
I (407) BM1088 Module: accel_read_rawY = -228.284729
I (407) BM1088 Module: accel_read_rawZ = -229.283112
I (617) BM1088 Module: accel_read_rawX = 1.723825
I (617) BM1088 Module: accel_read_rawY = -228.291916
I (617) BM1088 Module: accel_read_rawZ = -229.168182
I (817) BM1088 Module: accel_read_rawX = 1.766921
I (817) BM1088 Module: accel_read_rawY = -228.356567
I (817) BM1088 Module: accel_read_rawZ = -229.161026
I (1017) BM1088 Module: accel_read_rawX = 0.682347
I (1017) BM1088 Module: accel_read_rawY = -229.003006
I (1017) BM1088 Module: accel_read_rawZ = -229.448318
I (1217) BM1088 Module: accel_read_rawX = 0.624887
I (1217) BM1088 Module: accel_read_rawY = -228.722870
I (1217) BM1088 Module: accel_read_rawZ = -229.017365
I (1417) BM1088 Module: accel_read_rawX = 1.795651
I (1417) BM1088 Module: accel_read_rawY = -228.291916
I (1417) BM1088 Module: accel_read_rawZ = -229.225662
I (1617) BM1088 Module: accel_read_rawX = 0.215478
I (1617) BM1088 Module: accel_read_rawY = -229.139465
I (1617) BM1088 Module: accel_read_rawZ = -228.378113
I (1817) BM1088 Module: accel_read_rawX = 1.292869
I (1817) BM1088 Module: accel_read_rawY = -228.090790
I (1817) BM1088 Module: accel_read_rawZ = -228.234467


However, the output data I get does not make sense. What am I doing wrong?

[MicroController]

Code: Select all

ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), BM1088_ACCEL_ADDRESS, (&reg_addr + i), 1, ...

(&reg_addr + i) is not what you want, yielding a pointer to some undefined byte on the stack.
Try this:

Code: Select all

uint8_t ra = reg_addr + i;
ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), BM1088_ACCEL_ADDRESS, &ra, 1, ...

[nyameaama]

Thanks! that fixed the big discrepancy of the axis values.However it seems that my pitch roll and yaw calculations may be off as they jump massively even though I laid the PCBs flat on a surface. Do you have any thoughts as to what else I'm doing wrong?

I (338) main_task: Started on CPU0
I (348) main_task: Calling app_main()
I (348) BM1088 Module: I2C initialized successfully
I (348) BM1088 Module: GYRO_CHIP_ID_REGISTER_VALUE = F
I (408) BM1088 Module: Accelerometer configured successfully
I (408) BM1088 Module: accel_read_rawX = -0.114922
I (408) BM1088 Module: accel_read_rawY = -0.387861
I (408) BM1088 Module: accel_read_rawZ = 39.217022
I (418) BM1088 Module: pitch = 43.211248
I (418) BM1088 Module: roll = -133.100651
I (628) BM1088 Module: accel_read_rawX = -0.172383
I (628) BM1088 Module: accel_read_rawY = -0.344765
I (628) BM1088 Module: accel_read_rawZ = 39.303215
I (628) BM1088 Module: pitch = 28.000519
I (638) BM1088 Module: roll = -120.972667
I (838) BM1088 Module: accel_read_rawX = -0.158017
I (838) BM1088 Module: accel_read_rawY = -0.272939
I (838) BM1088 Module: accel_read_rawZ = 39.310398
I (838) BM1088 Module: pitch = 80.263620
I (848) BM1088 Module: roll = -176.499250
I (1048) BM1088 Module: accel_read_rawX = -0.265756
I (1048) BM1088 Module: accel_read_rawY = -0.395043
I (1048) BM1088 Module: accel_read_rawZ = 39.662346
I (1048) BM1088 Module: pitch = 42.406332
I (1058) BM1088 Module: roll = -114.655216
I (1258) BM1088 Module: accel_read_rawX = -0.014365
I (1258) BM1088 Module: accel_read_rawY = -0.359130
I (1258) BM1088 Module: accel_read_rawZ = 39.454048
I (1258) BM1088 Module: pitch = 1.045840
I (1268) BM1088 Module: roll = -97.503039
I (1468) BM1088 Module: accel_read_rawX = -0.179565
I (1468) BM1088 Module: accel_read_rawY = -0.330400
I (1468) BM1088 Module: accel_read_rawZ = 39.339127
I (1468) BM1088 Module: pitch = 24.467079
I (1478) BM1088 Module: roll = -119.300149
I (1678) BM1088 Module: accel_read_rawX = -0.014365
I (1678) BM1088 Module: accel_read_rawY = -0.366313
I (1678) BM1088 Module: accel_read_rawZ = 39.231388
I (1678) BM1088 Module: pitch = 9.587834
I (1688) BM1088 Module: roll = -109.863306
I (1888) BM1088 Module: accel_read_rawX = -0.129287
I (1888) BM1088 Module: accel_read_rawY = -0.272939
I (1888) BM1088 Module: accel_read_rawZ = 39.238571
I (1888) BM1088 Module: pitch = 52.756513
I (1898) BM1088 Module: roll = -130.387234
I (2098) BM1088 Module: accel_read_rawX = -0.143652
I (2098) BM1088 Module: accel_read_rawY = -0.416591
I (2098) BM1088 Module: accel_read_rawZ = 39.224205
I (2098) BM1088 Module: pitch = 50.564989
I (2108) BM1088 Module: roll = -96.347288
I (2308) BM1088 Module: accel_read_rawX = -0.150835
I (2308) BM1088 Module: accel_read_rawY = -0.430956
I (2308) BM1088 Module: accel_read_rawZ = 39.339127
I (2308) BM1088 Module: pitch = 38.791491
I (2318) BM1088 Module: roll = -139.408974
I (2518) BM1088 Module: accel_read_rawX = -0.114922
I (2518) BM1088 Module: accel_read_rawY = -0.316035
I (2518) BM1088 Module: accel_read_rawZ = 39.173927
I (2518) BM1088 Module: pitch = 49.727083
I (2528) BM1088 Module: roll = -149.545470
I (2728) BM1088 Module: accel_read_rawX = -0.193930
I (2728) BM1088 Module: accel_read_rawY = -0.409409
I (2728) BM1088 Module: accel_read_rawZ = 39.224205
I (2728) BM1088 Module: pitch = 72.648343
I (2738) BM1088 Module: roll = -138.407327
I (2938) BM1088 Module: accel_read_rawX = -0.136469
I (2938) BM1088 Module: accel_read_rawY = -0.359130
I (2938) BM1088 Module: accel_read_rawZ = 39.260117
I (2938) BM1088 Module: pitch = 3.758720
I (2948) BM1088 Module: roll = -175.853310
I (3148) BM1088 Module: accel_read_rawX = -0.050278
I (3148) BM1088 Module: accel_read_rawY = -0.423774
I (3148) BM1088 Module: accel_read_rawZ = 39.217022
I (3148) BM1088 Module: pitch = 2.460931
I (3158) BM1088 Module: roll = -128.793851
I (3358) BM1088 Module: accel_read_rawX = -0.143652
I (3358) BM1088 Module: accel_read_rawY = -0.359130
I (3358) BM1088 Module: accel_read_rawZ = 39.209843
I (3358) BM1088 Module: pitch = 56.679845
I (3368) BM1088 Module: roll = -154.036952
I (3568) BM1088 Module: accel_read_rawX = -0.079009
I (3568) BM1088 Module: accel_read_rawY = -0.380678
I (3568) BM1088 Module: accel_read_rawZ = 39.260117
I (3568) BM1088 Module: pitch = 59.431913
I (3578) BM1088 Module: roll = -107.361932
I (3778) BM1088 Module: accel_read_rawX = -0.021548
I (3778) BM1088 Module: accel_read_rawY = -0.280122
I (3778) BM1088 Module: accel_read_rawZ = 39.123650
I (3778) BM1088 Module: pitch = 7.579353
I (3788) BM1088 Module: roll = -125.226817
I (3988) BM1088 Module: accel_read_rawX = -0.100556
I (3988) BM1088 Module: accel_read_rawY = -0.373495
I (3988) BM1088 Module: accel_read_rawZ = 39.188293
I (3988) BM1088 Module: pitch = 43.838692
I (3998) BM1088 Module: roll = -137.736456
I (4198) BM1088 Module: accel_read_rawX = 4.108450
I (4198) BM1088 Module: accel_read_rawY = -2.126051
I (4198) BM1088 Module: accel_read_rawZ = 37.550659
I (4198) BM1088 Module: pitch = 39.351621
I (4208) BM1088 Module: roll = -161.576952
I (4408) BM1088 Module: accel_read_rawX = -6.040571
I (4408) BM1088 Module: accel_read_rawY = -3.576937
I (4408) BM1088 Module: accel_read_rawZ = 38.922539

[MicroController]

Did you fix the (&reg_addr + i) in bm1088_gyro_read() too?

Btw, the BMI088 does "auto increment" on the register address, so instead of

Code: Select all

for (size_t i = 0; i < len; i++) {
  uint8_t ra = reg_addr + i;
  ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), BM1088_ACCEL_ADDRESS, &ra, 1,
  (data + i), 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS );
}

it would be up to 4x faster (and easier) to read all the data you want in one go:

Code: Select all

ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), BM1088_ACCEL_ADDRESS, &reg_addr, 1,
  data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS );

This transfers 3+len bytes across the bus instead of len*(3+1).

[MicroController]

And, that half_scale part doesn't look right.
Here's an alternative suggestion:

Code: Select all

esp_err_t BMI088::read_value(const uint8_t addr, const mpu_register_address lsb_reg, const uint16_t scale, float& out) {
  int16_t raw;
  const esp_err_t ret = i2c_master_write_read_device(i2c_port_t(I2C_MASTER_NUM), addr, &lsb_reg, 1, (uint8_t*)&raw, sizeof(raw), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS );
  if(ret == ESP_OK) {
    const int32_t fixedpoint = (int32_t)raw * scale;
    out = (float)fixedpoint / 32768;
  }
  return ret;  
}

...

float BMI088_IMU::accel_read_X_mps2() {
  const uint16_t acc_range = 24; // 24g full-scale
  float acc_gs;
  const esp_err_t ret = read_value(BM1088_ACCEL_ADDRESS, ACC_X_LSB, acc_range, acc_gs);
  if(ret == ESP_OK) {
    // acc_gs now contains the X acceleration in G's
    return acc_gs * GRAVITY_EARTH;
  } else {
    // Error handling, exception, or whatever...
    return 0.0f;
  }
}
...

[nyameaama]

Thanks, I made the changes you requested and this improved it a lot. I still had problems with pitch roll and yaw however after a lot of combing through I realized I was using gyroscope values instead of accelerometer values for the pitch and roll. So I modified this:
(USING GYROSCOPE)
double BMI088_IMU::angle_read_pitch(){
double x_Buff = gyro_read_rawX();
double y_Buff = gyro_read_rawY();
double z_Buff = gyro_read_rawZ();
double pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
return pitch;
}

double BMI088_IMU::angle_read_roll(){
double y_Buff = gyro_read_rawY();
double z_Buff = gyro_read_rawZ();
double roll = atan2(y_Buff , z_Buff) * 57.3;
return roll;
}

To this:
(USING ACCELEROMTETER)
double BMI088_IMU::angle_read_pitch(){
double x_Buff = accel_read_rawX();
double y_Buff = accel_read_rawY();
double z_Buff = accel_read_rawZ();
double pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
return pitch;
}

double BMI088_IMU::angle_read_roll(){
double y_Buff = accel_read_rawY();
double z_Buff = accel_read_rawZ();
double roll = atan2(y_Buff , z_Buff) * 57.3;
return roll;
}

This is some of the output data. There are no more excessive jumps (other than the torque I put on it) between each pitch and roll data reading:
I (43648) BM1088 Module: accel_read_rawX = 5.056554
I (43648) BM1088 Module: accel_read_rawY = 4.302381
I (43648) BM1088 Module: accel_read_rawZ = 38.168365
I (43648) BM1088 Module: gyro_read_rawX = 1.220703
I (43658) BM1088 Module: gyro_read_rawY = 0.778198
I (43658) BM1088 Module: gyro_read_rawZ = -1.174927
I (43668) BM1088 Module: pitch = -3.722616
I (43668) BM1088 Module: roll = 8.968992
I (43878) BM1088 Module: accel_read_rawX = 0.660800
I (43878) BM1088 Module: accel_read_rawY = 10.378864
I (43878) BM1088 Module: accel_read_rawZ = 40.452431
I (43878) BM1088 Module: gyro_read_rawX = 4.608154
I (43888) BM1088 Module: gyro_read_rawY = -2.029419
I (43888) BM1088 Module: gyro_read_rawZ = 1.327515
I (43898) BM1088 Module: pitch = -6.031060
I (43898) BM1088 Module: roll = 14.593245
I (44108) BM1088 Module: accel_read_rawX = 5.049372
I (44108) BM1088 Module: accel_read_rawY = 14.243107
I (44108) BM1088 Module: accel_read_rawZ = 34.727894
I (44108) BM1088 Module: gyro_read_rawX = 6.454468
I (44118) BM1088 Module: gyro_read_rawY = -0.549316
I (44118) BM1088 Module: gyro_read_rawZ = -1.281738
I (44128) BM1088 Module: pitch = -6.839497
I (44128) BM1088 Module: roll = 24.001851
I (44338) BM1088 Module: accel_read_rawX = 5.458780
I (44338) BM1088 Module: accel_read_rawY = 20.362686
I (44338) BM1088 Module: accel_read_rawZ = 32.271442
I (44338) BM1088 Module: gyro_read_rawX = 1.647949
I (44348) BM1088 Module: gyro_read_rawY = 0.671387
I (44348) BM1088 Module: gyro_read_rawZ = -3.021240
I (44358) BM1088 Module: pitch = -6.215660
I (44358) BM1088 Module: roll = 31.168653
I (44568) BM1088 Module: accel_read_rawX = 4.596867
I (44568) BM1088 Module: accel_read_rawY = 23.343468
I (44568) BM1088 Module: accel_read_rawZ = 32.034420
I (44568) BM1088 Module: gyro_read_rawX = 2.609253
I (44578) BM1088 Module: gyro_read_rawY = 0.473022
I (44578) BM1088 Module: gyro_read_rawZ = 0.778198
I (44588) BM1088 Module: pitch = -3.989989
I (44588) BM1088 Module: roll = 34.751520
I (44798) BM1088 Module: accel_read_rawX = 5.307945
I (44798) BM1088 Module: accel_read_rawY = 25.807098
I (44798) BM1088 Module: accel_read_rawZ = 29.319393
I (44798) BM1088 Module: gyro_read_rawX = 0.778198
I (44808) BM1088 Module: gyro_read_rawY = -1.007080
I (44808) BM1088 Module: gyro_read_rawZ = 1.708984
I (44818) BM1088 Module: pitch = -6.630353
I (44818) BM1088 Module: roll = 40.365892
I (45028) BM1088 Module: accel_read_rawX = 4.747702
I (45028) BM1088 Module: accel_read_rawY = 25.045744
I (45028) BM1088 Module: accel_read_rawZ = 28.895618
I (45028) BM1088 Module: gyro_read_rawX = 0.259399
I (45038) BM1088 Module: gyro_read_rawY = 0.885010
I (45038) BM1088 Module: gyro_read_rawZ = -0.213623
I (45048) BM1088 Module: pitch = -6.796082
I (45048) BM1088 Module: roll = 42.016977
I (45258) BM1088 Module: accel_read_rawX = 4.388572
I (45258) BM1088 Module: accel_read_rawY = 26.575638
I (45258) BM1088 Module: accel_read_rawZ = 29.017725
I (45258) BM1088 Module: gyro_read_rawX = 0.762939
I (45268) BM1088 Module: gyro_read_rawY = 0.091553
I (45268) BM1088 Module: gyro_read_rawZ = -0.305176
I (45278) BM1088 Module: pitch = -6.166298
I (45278) BM1088 Module: roll = 41.803262
I (45488) BM1088 Module: accel_read_rawX = 4.273650
I (45488) BM1088 Module: accel_read_rawY = 26.180595
I (45488) BM1088 Module: accel_read_rawZ = 29.348125
I (45488) BM1088 Module: gyro_read_rawX = -0.900269
I (45498) BM1088 Module: gyro_read_rawY = 0.717163
I (45498) BM1088 Module: gyro_read_rawZ = 0.183105
I (45508) BM1088 Module: pitch = -6.141952
I (45508) BM1088 Module: roll = 40.452803
I (45718) BM1088 Module: accel_read_rawX = 4.769250
I (45718) BM1088 Module: accel_read_rawY = 23.673866
I (45718) BM1088 Module: accel_read_rawZ = 29.958647
I (45718) BM1088 Module: gyro_read_rawX = -2.731323
I (45728) BM1088 Module: gyro_read_rawY = -0.427246
I (45728) BM1088 Module: gyro_read_rawZ = -0.335693
I (45738) BM1088 Module: pitch = -7.881747
I (45738) BM1088 Module: roll = 38.422626
I (45948) BM1088 Module: accel_read_rawX = 4.223372
I (45948) BM1088 Module: accel_read_rawY = 18.358738
I (45948) BM1088 Module: accel_read_rawZ = 33.557129
I (45948) BM1088 Module: gyro_read_rawX = -7.522583
I (45958) BM1088 Module: gyro_read_rawY = 0.900269
I (45958) BM1088 Module: gyro_read_rawZ = 0.717163
I (45968) BM1088 Module: pitch = -6.124104
I (45968) BM1088 Module: roll = 28.189816
I (46178) BM1088 Module: accel_read_rawX = 4.187459
I (46178) BM1088 Module: accel_read_rawY = 13.869610
I (46178) BM1088 Module: accel_read_rawZ = 35.905846
I (46178) BM1088 Module: gyro_read_rawX = -4.455566
I (46188) BM1088 Module: gyro_read_rawY = 0.610352
I (46188) BM1088 Module: gyro_read_rawZ = 3.692627
I (46198) BM1088 Module: pitch = -6.270813
I (46198) BM1088 Module: roll = 19.751971
I (46408) BM1088 Module: accel_read_rawX = 2.240973
I (46408) BM1088 Module: accel_read_rawY = 6.557718
I (46408) BM1088 Module: accel_read_rawZ = 36.961685
I (46408) BM1088 Module: gyro_read_rawX = -3.631592
I (46418) BM1088 Module: gyro_read_rawY = 1.220703
I (46418) BM1088 Module: gyro_read_rawZ = 0.717163
I (46428) BM1088 Module: pitch = -4.633506
I (46428) BM1088 Module: roll = 12.883423
I (46638) BM1088 Module: accel_read_rawX = 2.944868
I (46638) BM1088 Module: accel_read_rawY = 4.697424
I (46638) BM1088 Module: accel_read_rawZ = 38.491581
I (46638) BM1088 Module: gyro_read_rawX = -1.449585
I (46648) BM1088 Module: gyro_read_rawY = 1.220703
I (46648) BM1088 Module: gyro_read_rawZ = -0.717163
I (46658) BM1088 Module: pitch = -4.749686
I (46658) BM1088 Module: roll = 8.940107
I (46868) BM1088 Module: accel_read_rawX = 1.149217
I (46868) BM1088 Module: accel_read_rawY = -2.355895
I (46868) BM1088 Module: accel_read_rawZ = 36.724659
I (46868) BM1088 Module: gyro_read_rawX = -2.212524
I (46878) BM1088 Module: gyro_read_rawY = 2.731323
I (46878) BM1088 Module: gyro_read_rawZ = 0.579834
I (46888) BM1088 Module: pitch = -3.048920
I (46888) BM1088 Module: roll = 0.149400
I (47098) BM1088 Module: accel_read_rawX = -4.187459
I (47098) BM1088 Module: accel_read_rawY = 1.055843
I (47098) BM1088 Module: accel_read_rawZ = 38.635235
I (47098) BM1088 Module: gyro_read_rawX = 0.289917
I (47108) BM1088 Module: gyro_read_rawY = 1.739502
I (47108) BM1088 Module: gyro_read_rawZ = 0.503540
I (47118) BM1088 Module: pitch = 3.022574
I (47118) BM1088 Module: roll = -6.843277
I (47328) BM1088 Module: accel_read_rawX = -3.325546
I (47328) BM1088 Module: accel_read_rawY = -3.117250
I (47328) BM1088 Module: accel_read_rawZ = 39.870640
I (47328) BM1088 Module: gyro_read_rawX = 0.839233
I (47338) BM1088 Module: gyro_read_rawY = -1.434326
I (47338) BM1088 Module: gyro_read_rawZ = -0.274658
I (47348) BM1088 Module: pitch = 5.797258
I (47348) BM1088 Module: roll = -3.444648
I (47558) BM1088 Module: accel_read_rawX = -2.801216
I (47558) BM1088 Module: accel_read_rawY = -1.307234
I (47558) BM1088 Module: accel_read_rawZ = 39.044640
I (47558) BM1088 Module: gyro_read_rawX = 0.350952
I (47568) BM1088 Module: gyro_read_rawY = 0.274658
I (47568) BM1088 Module: gyro_read_rawZ = 0.335693
I (47578) BM1088 Module: pitch = 3.811644
I (47578) BM1088 Module: roll = -1.844653