How to Convert BMI160 Data into roll, pitch and yaw

[amar]

Hi everyone,

Present i am working with BMX160, Till i am able to read Three sensor's raw data(Acc, Gyro, Mag) and convert them into Gravity, Radiuns/sec,uT respectively and To converting these readings iinto roll, pitch, yaw i am using below section of code(i got this from google)like below.

#define   ACCELEROMETER_SENSITIVITY       16384.0
#define  GYROSCOPE_SENSITIVITY                  16.4 //from bmx160 datasheet pg no:9,it is the value form range FS2000
#define   M_PI                                                                3.14159265359
#define   dt                                                                      0.01   // 10 ms sample rate!

void ComplementaryFilter(int16_t *accData, int16_t *gyrData, float *pitch, float *roll)
{
float pitchAcc, rollAcc, yaw = 0.0;
float *yaw_ptr;

// Integrate the gyroscope data -> int(angularSpeed) = angle
*pitch = ( (float)gyro.x / GYROSCOPE_SENSITIVITY ) * dt; // Angle around the X-axis
*roll = ( (float)gyro.y / GYROSCOPE_SENSITIVITY ) * dt; // Angle around the Y-axis

yaw = yaw + ((float)gyro.z) * dt; // Angle around the Z-axis
yaw_ptr = &yaw;

// Compensate for drift with accelerometer data if !bull**bleep**
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
int forceMagnitudeApprox = abs(accel.x) + abs(accel.y) + abs(accel.z);

if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)accel.y, (float)accel.z) * 180 / M_PI;
*pitch = *pitch * 0.98 + pitchAcc * 0.02;

// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f((float)accel.x, (float)accel.z) * 180 / M_PI;
*roll = *roll * 0.98 + rollAcc * 0.02;
}

PRINTF("pitch\t%0.5f\troll\t%0.5f\tyaw\t%0.5f\t\r\n", *pitch,*roll, yaw);
}

Can you please review it once.

If it is wrong to proceede so please correct me and suggeat me right  formula for converting it.

Thank you inadvance,

amarr 

[Yanchao]

Hi,

We have formula as bellow:

#define GRAVITY_EARTH   (9.80665f) /* Earth's gravity in m/s^2 */
#define RAD             (57.2957805f)
#define INV_RAD         (0.01745329f)

/*!
 *  @brief This internal API is used to convert lsb to ms2 
 *
 *  @param[in] val: value
 *  @param[in] range: range value
 *  @param[in] bit_width
 *
 *  @return float value in ms2
 *
 */
float lsb_to_ms2(int16_t val, float g_range, uint8_t bit_width)
{
	float half_scale = (float)(1 << bit_width) / 2.0f;

	return GRAVITY_EARTH * val * g_range / half_scale;
}
float lsb_to_mg(int16_t val, float g_range, uint8_t bit_width)
{
	float half_scale = (float)(1 << bit_width) / 2.0f;

	return 1000.0f * val * g_range / half_scale;
}

float lsb_to_dps(int16_t val, float g_range, uint8_t bit_width)
{
	float half_scale = (float)(1 << bit_width) / 2.0f;

	return val * g_range / half_scale;
}

example: 
accel_x = lsb_to_ms2(accel_data[i].x, 8, 16);
gyro_x = lsb_to_dps(gyro_data[i].x, 2000, 16);

#define MAG_FRAMES	20
struct bmm150_mag_data mag_data[MAG_FRAMES];
rslt = bmm150_aux_mag_data(aux_data[i].data, &bmm150dev);
mag_data[i].x = bmm150dev.data.x;
mag_data[i].y = bmm150dev.data.y;
mag_data[i].z = bmm150dev.data.z;