02-01-2021 07:57 PM
Hi everyone,
I worked on BMX160 IMU last few days, Till now i am able to get Raw readings of it like ACC(X,Y,Z),GYRO (X,Y,Z)and MAG(X,Y,Z) sensor.
I confirmed raw readings of ACC and GYRO are correct or not by placing sensor at rest state and then raw readings are came like acc: 0,0,-/+1 and
gyro: 0,0,0.(datasheet page no:106) but i didnot find any thing which is related to confirm Raw MAG readings are correct or not?
And also i converted Acc and Gyro raw readings in to gravity,m/s^2 by using below formulas,
//Here 2 is for 2g and 32768 for 16 bit value because ACC raw data is in 16 bit value
Ax = (acc_raw_x * 2)/32768;
Ay = (acc_raw_y * 2)/32768;
Az = (acc_raw_z * 2)/32768;
//Here 250 is for 250dps and 32768 for 16 bit value because Gyro raw data is in 16 bit value
Gx = (gyro_raw_x * 250)/32768;
Gy = (gyro_raw_y * 250)/32768;
Gz = (gyro_raw_z * 250)/32768;
1) Can you please help me to find MAG Raw data correct or not?
2) I am just reading raw data from six registers of MAG(0x04 -0x09)?is it correct?
3) In BMX160 Datasheet for reading MAG data procedure given like below attached imge
4) What is bmi160 API for doing above image task? i did not find any API in bmi160.c file like above image suggested ?
5) What king of fusion algorithm/filters can i use to find pitch,roll and yaw from above readings?
Please help me to come out of this issue.
Thank you,
Amarr
Solved! Go to Solution.
02-09-2021 10:37 AM
Hi Minhwan,
Thank you for reply,
Till now I am following below atached file .c and .h file for BMX160 to reading raw data(It is based on arduino). In that source codes they are not doing any temparature compansation like bosch image(I attached early), Can I get correct readings without temparature compansation or should I implement temparature compansation?
bmx160.c:-
void BMI160Class::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy,
int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz, int16_t* rh) { //Added for BMM150 Support
uint8_t buffer[20];
buffer[0] = BMI160_RA_MAG_X_L;
serial_buffer_transfer(buffer, 1, 20);
*mx = (((int16_t)buffer[1]) << 😎 | buffer[0];
*my = (((int16_t)buffer[3]) << 😎 | buffer[2];
*mz = (((int16_t)buffer[5]) << 😎 | buffer[4];
*rh = (((int16_t)buffer[7]) << 😎 | buffer[6];
*gx = (((int16_t)buffer[9]) << 😎 | buffer[8];
*gy = (((int16_t)buffer[11]) << 😎 | buffer[10];
*gz = (((int16_t)buffer[13]) << 😎 | buffer[12];
*ax = (((int16_t)buffer[15]) << 😎 | buffer[14];
*ay = (((int16_t)buffer[17]) << 😎 | buffer[16];
*az = (((int16_t)buffer[19]) << 😎 | buffer[18];
}
CurieIMU.c
void CurieIMUClass::readMotionSensor9(int& ax, int& ay, int& az, int& gx, int& gy, int& gz, int& mx, int& my, int& mz, int& rh) //Added for BMM150 Support
{
int16_t sax, say, saz, sgx, sgy, sgz, smx, smy, smz, srh;
getMotion9(&sax, &say, &saz, &sgx, &sgy, &sgz, &smx, &smy, &smz, &srh);
ax = sax;
ay = say;
az = saz;
gx = sgx;
gy = sgy;
gz = sgz;
mx = smx;
my = smy;
mz = smz;
rh = srh;
}
arduino.ino
#include <BMI160.h>
#include <BMI160Gen.h>
#include <CurieIMU.h>
/*
* Copyright (c) 2016 Intel Corporation. All rights reserved.
* See the bottom of this file for the license terms.
*/
/*
This sketch example demonstrates how the BMI160 on the
Intel(R) Curie(TM) module can be used to read gyroscope data
*/
#include <BMI160Gen.h>
// Definitions
#define sampleFreqDef 512.0f // sample frequency in Hz
#define betaDef 0.1f // 2 * proportional gain
double delta_t = 0; // Used to control display output rate
uint32_t now = 0; // used to calculate integration interval
uint32_t last_update = 0; // used to calculate integration interval
float beta; // algorithm gain
float q0;
float q1;
float q2;
float q3; // quaternion of sensor frame relative to auxiliary frame
float roll;
float pitch;
float yaw;
char anglesComputed;
// AHRS algorithm update
void Madgwick_BMX160_Madgwick_BMX160();
// IMU algorithm update
void Madgwick_BMX160_updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
// Fast inverse square-root
float Madgwick_BMX160_invSqrt(float x);
void Madgwick_BMX160_computeAngles();
float getRoll();
float getPitch();
float getYaw();
float getRollRadians();
float getPitchRadians();
float getYawRadians();
void getQuaternion(float *w, float *x, float *y, float *z);
void setup() {
Serial.begin(115200); // initialize Serial communication
while (!Serial); // wait for the serial port to open
// initialize device
BMI160.begin(10);
uint8_t dev_id = BMI160.getDeviceID();
Serial.print("DEVICE ID: ");
Serial.println(dev_id, HEX);
// Set the accelerometer range to 250 degrees/second
BMI160.setGyroRange(BMI160_GYRO_RANGE_250);
BMI160.setGyroRate(BMI160_GYRO_RATE_100HZ);
BMI160.setAccelerometerRange(2);
BMI160.setAccelerometerRate(BMI160_ACCEL_RATE_100HZ);
Madgwick_BMX160_Madgwick_BMX160();
}
void loop()
{
int axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, mxRaw, myRaw, mzRaw, tRaw;
float ax, ay, az, gx, gy, gz;
// read raw gyro measurements from device
BMI160.readGyro(gxRaw, gyRaw, gzRaw);
BMI160.readAccelerometer(axRaw, ayRaw, azRaw);
BMI160.getMagneto( mxRaw, myRaw, mzRaw);
BMI160.readMotionSensor9(axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, mxRaw, myRaw, mzRaw, tRaw);
// convert the raw gyro data to degrees/second
gx = convertRawGyro(gxRaw);
gy = convertRawGyro(gyRaw);
gz = convertRawGyro(gzRaw);
// convert the raw gyro data to degrees/second
ax = convertRawAccel(axRaw);
ay = convertRawAccel(ayRaw);
az = convertRawAccel(azRaw);
Madgwick_BMX160_update(gx, gy, gz, ax, ay, az, mxRaw, myRaw, mzRaw);
getRoll();
getPitch();
getYaw();
Serial.print(axRaw);
Serial.print(",");
Serial.print(ayRaw);
Serial.print(",");
Serial.print(azRaw);
Serial.print(",");
Serial.print(gxRaw);
Serial.print(",");
Serial.print(gyRaw);
Serial.print(",");
Serial.print(gzRaw);
Serial.print(",");
Serial.print(mxRaw);
Serial.print(",");
Serial.print(myRaw);
Serial.print(",");
Serial.print(mzRaw);
Serial.print(",");
Serial.print(ax);
Serial.print(",");
Serial.print(ay);
Serial.print(",");
Serial.print(az);
Serial.print(",");
Serial.print(gx);
Serial.print(",");
Serial.print(gy);
Serial.print(",");
Serial.print(gz);
Serial.print(",");
Serial.print(roll);
Serial.print(",");
Serial.print(pitch);
Serial.print(",");
Serial.print(yaw);
Serial.print(",");
Serial.println();
delay(10);
}
float convertRawGyro(int gRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float g = (gRaw * 250.0) / 32768.0;
return g;
}
float convertRawAccel(int aRaw) {
// since we are using 250 degrees/seconds range
// -250 maps to a raw value of -32768
// +250 maps to a raw value of 32767
float a = (aRaw * (2/32768.0));
return a;
}
void Madgwick_BMX160_Madgwick_BMX160() {
beta = betaDef;
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
now = micros();
anglesComputed = 0;
}
void Madgwick_BMX160_update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
Madgwick_BMX160_updateIMU(gx, gy, gz, ax, ay, az);
return;
}
now = micros();
// Set integration time by time elapsed since last filter update
delta_t = ((now - last_update) / 1000000.0f);
last_update = now;
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = Madgwick_BMX160_invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Normalise magnetometer measurement
recipNorm = Madgwick_BMX160_invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrtf(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = Madgwick_BMX160_invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * delta_t;
q1 += qDot2 * delta_t;
q2 += qDot3 * delta_t;
q3 += qDot4 * delta_t;
// Normalise quaternion
recipNorm = Madgwick_BMX160_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
anglesComputed = 0;
}
void Madgwick_BMX160_updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
now = micros();
// Set integration time by time elapsed since last filter update
delta_t = ((now - last_update) / 1000000.0f);
last_update = now;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = Madgwick_BMX160_invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = Madgwick_BMX160_invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * delta_t;
q1 += qDot2 * delta_t;
q2 += qDot3 * delta_t;
q3 += qDot4 * delta_t;
// Normalise quaternion
recipNorm = Madgwick_BMX160_invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
anglesComputed = 0;
}
float Madgwick_BMX160_invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
}
void Madgwick_BMX160_computeAngles()
{
roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
pitch = asinf(-2.0f * (q1*q3 - q0*q2));
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
anglesComputed = 1;
}
float getRoll() {
if (!anglesComputed) Madgwick_BMX160_computeAngles();
return roll * 57.29578f;
}
float getPitch() {
if (!anglesComputed) Madgwick_BMX160_computeAngles();
return pitch * 57.29578f;
}
float getYaw() {
if (!anglesComputed) Madgwick_BMX160_computeAngles();
return yaw * 57.29578f + 180.0f;
}
float getRollRadians() {
if (!anglesComputed) Madgwick_BMX160_computeAngles();
return roll;
}
float getPitchRadians() {
if (!anglesComputed) Madgwick_BMX160_computeAngles();
return pitch;
}
float getYawRadians() {
if (!anglesComputed) Madgwick_BMX160_computeAngles();
return yaw;
}
void getQuaternion(float *w, float *x, float *y, float *z) {
*w = q0;
*x = q1;
*y = q2;
*z = q3;
}
From above three source codes i am usong below API for reading MAG raw data along with ACC, GYRO also.
arduino.ino sktech is the main application code and remaining are back end driver files.
1) BMI160.readMotionSensor9(axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, mxRaw, myRaw, mzRaw, tRaw); ---->for reading raw data not doing temprature compansation
2) Madgwick_BMX160_update(gx, gy, gz, ax, ay, az, mxRaw, myRaw, mzRaw); --->for converting raw data ino pitch,roll,yaw
Thank you,
amarr
02-10-2021 03:57 AM
Hello Amar,
I don't think your code is bosch made.
Let me explain based on our github source.
Here is the our BMX160 source code as below. ( There is explanation about BMX160 in BMI160 github)
https://github.com/BoschSensortec/BMI160_driver
And when you call bmm150_aux_mag_data, there is compensation process.
Then, you can get compensated data.
Thanks,