Hello, I am using the BMI160 sensor with NodeMcu over I2C. The project that i'm trying to do is, obtaining the roll and pitch values from the sensors. After i get roll and pitch values from sensors i'll obtain the inclination. My problem is, values that the sensor provide me are not stable, even if i doesn't move the BMI160, there can be inconsistency between obtained values. For example in the first read for the roll value is "-12.62" and in the second read the value is "-12.55". So i wonder if those values are fine according to sensitivity of the sensor or am I doing something wrong, also are there anything that i can do for keep the values more stable? Please see the used code below (The code is derived from the : https://makers-with-myson.blog.ss-blog.jp/2020-06-14). #include <Wire.h>
#include <math.h>
#include <BMI160Gen.h>
#define PRINT_PROCESSING
#define BAUDRATE 9600
#define SENSE_RATE 100
#define GYRO_RANGE 2000
#define ACCL_RANGE 16
#define SENSOR1 0x69
#define deg_to_rad(a) (a/180*M_PI)
#define rad_to_deg(a) (a/M_PI*180)
float convertRawGyro(int gRaw) {
// ex) if the range is +/-500 deg/s: +/-32768/500 = +/-65.536 LSB/(deg/s)
float lsb_omega = float(0x7FFF) / GYRO_RANGE;
return gRaw / lsb_omega; // deg/sec
}
float convertRawAccel(int aRaw) {
// ex) if the range is +/-2g ; +/-32768/2 = +/-16384 LSB/g
float lsb_g = float(0x7FFF) / ACCL_RANGE;
return aRaw / lsb_g;
}
static float gyro_roll = 0, gyro_pitch = 0, gyro_yaw = 0;
static float comp_roll = 0, comp_pitch = 0;
static unsigned long last_mills = 0;
void print_roll_pitch()
{
// read raw accl measurements from device
int rawXAcc, rawYAcc, rawZAcc; // x, y, z
BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
float accX = convertRawAccel(rawXAcc);
float accY = convertRawAccel(rawYAcc);
float accZ = convertRawAccel(rawZAcc);
float rad_a_roll = atan2(accY, accZ);
float rad_a_pitch = atan2(-accX, sqrt(accY*accY + accZ*accZ));
float accl_roll = rad_to_deg(rad_a_roll);
float accl_pitch = rad_to_deg(rad_a_pitch);
// read raw gyro measurements from device
int rawRoll, rawPitch, rawYaw; // roll, pitch, yaw
BMI160.readGyro(rawRoll, rawPitch, rawYaw);
float omega_roll = convertRawGyro(rawRoll);
float omega_pitch = convertRawGyro(rawPitch);
float omega_yaw = convertRawGyro(rawYaw);
unsigned long cur_mills = micros();
unsigned long duration = cur_mills - last_mills;
last_mills = cur_mills;
double dt = duration / 1000000.0; // us->s
if (dt > 0.1) return;
// Gyro data
gyro_roll += omega_roll * dt; // (ms->s) omega x time = degree
gyro_pitch += omega_pitch * dt;
gyro_yaw += omega_yaw * dt;
// Complementary filter data
comp_roll = 0.93 * (comp_roll + omega_roll * dt) + 0.07 * accl_roll;
comp_pitch = 0.93 * (comp_pitch + omega_pitch * dt) + 0.07 * accl_pitch;
#ifdef PRINT_PROCESSING
static int n = 0;
if (n != 50) {
++n; return;
}
n = 0;
Serial.print(comp_roll);
Serial.print(",");
Serial.print(comp_pitch);
Serial.print(",");
#endif
Serial.println();
}
void setup() {
Serial.begin(BAUDRATE);
BMI160.begin(BMI160GenClass::I2C_MODE);
BMI160.setGyroRate(SENSE_RATE);
BMI160.setAccelerometerRate(SENSE_RATE);
BMI160.setGyroRange(GYRO_RANGE);
BMI160.setAccelerometerRange(ACCL_RANGE);
BMI160.begin(BMI160GenClass::I2C_MODE, SENSOR1);
BMI160.setXAccelOffset(0);
BMI160.setYAccelOffset(0);
BMI160.setZAccelOffset(0);
BMI160.setXGyroOffset(0);
BMI160.setYGyroOffset(0);
BMI160.setZGyroOffset(0);
delay(100);
/* Set kalman and gyro starting angle */
int rawXAcc, rawYAcc, rawZAcc;
BMI160.readAccelerometer(rawXAcc, rawYAcc, rawZAcc);
float accX = convertRawAccel(rawXAcc) + 0.01;
float accY = convertRawAccel(rawYAcc) + 0.01;
float accZ = convertRawAccel(rawZAcc) + 0.01;
float roll = rad_to_deg(atan(accY / accZ));
float pitch = rad_to_deg(atan(-accX / sqrt(accY * accY + accZ * accZ)));
// Set starting angle
gyro_roll = comp_roll = roll;
gyro_pitch = gyro_pitch = pitch;
}
void loop() {
print_roll_pitch();
} Any kind of information is appreciated Thanks. Mehmet
... View more