I'm using the mpu6050 to read the data from accel and gyros and use them together with a complementary filter to get roll, pitch and yaw. My problem is that the accel readings are wrong and I don't understand if the sensor is not working properly or something in my code is wrong. In standstill I have reasonable value but when I tilt my robot the gyros still gives me reasonable values whereas the accel value are always in the range [-1;1].
- Robot standstill: aX: 0.06 - aY: 0.00 - aZ: 1.06 - gX: -0.11 - gY: 0.00 - gZ: -0.03 - Roll: -0.04 - Pitch: -0.00 - Yaw: -0.01
- Tilt to the right: aX: 0.39 - aY: 0.92 - aZ: 1.12 - gX: 4.36 - gY: 0.73 - gZ: -1.35 - Roll: 8.29 - Pitch: 1.39 - Yaw: -2.62
As you can see the Accel readings remain in a range that's not correct. I hope to get some clarification.
Below the code:
#include <arduino.h>
#include <MPU6050.h>
#include <wire.h>
static double previousTime = 0;
struct accel_data{
int16_t tmp_x = 0;
int16_t tmp_y = 0;
int16_t tmp_z = 0;
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
};
struct gyros_data {
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
int16_t tmp_x = 0;
int16_t tmp_y = 0;
int16_t tmp_z = 0;
};
struct accel_gyros{
float gx = 0;
float gy = 0;
float gz = 0;
float ax = 0;
float ay = 0;
float az = 0;
float roll = 0;
float pitch = 0;
float yaw = 0;
};
class MPU6050_obj{
public:
accel_data a_data;
gyros_data g_data;
accel_gyros data;
void calibration(const motor_setup&, accel_data& , gyros_data&, MPU6050&);
void data_processing(const accel_data& ,const gyros_data&, accel_gyros&);
};
MPU6050_obj myMPU;
MPU6050 mpu_lib;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu_lib.initialize();
myMPU.calibration(App_motor, myMPU.a_data, myMPU.g_data, mpu_lib);
}
void loop() {
if(irrecv.decode(&results)){
if(static_cast<button>(results.value) == button::unknown){
Serial.println("Ignoring erroneous signal");
irrecv.resume();
return;
}
switch (button_prs) {
case button::three: //mpu
myMPU.data_processing(myMPU.a_data, myMPU.g_data, myMPU.data);
break;
case button::five:
//something
break;
default:
break;
}
irrecv.resume();
}
}
void MPU6050_obj::calibration(const motor_setup& App_motor, accel_data& a_data, gyros_data& g_data, MPU6050& mpu_lib){ //There are better ways to implement this fcn
int16_t accel_x, accel_y, accel_z, gyros_x, gyros_y, gyros_z;
int n = 250, i = 0;
// Set gyroscope sensitivity to ±250 dps (131)
mpu_lib.setFullScaleGyroRange(MPU6050_GYRO_FS_250); //I'm using ±250dps and ±2G because of my robot behaviour (small turns, flat ground mostly, no rapid accelerations)
// Set accelerometer sensitivity to ±2 G (16384)
mpu_lib.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
//reading_test();
if(mpu_lib.testConnection() && App_motor.get_pin() == LOW){ //add this piece of code beacuse cal has to happen in standstill condition
for (i = 0; i < n ; i++){
mpu_lib.getAcceleration(&accel_x, &accel_y, &accel_z);
mpu_lib.getRotation(&gyros_x, &gyros_y, &gyros_z);
a_data.tmp_x += accel_x;
a_data.tmp_y += accel_y;
a_data.tmp_z += accel_z;
g_data.tmp_x += gyros_x;
g_data.tmp_y += gyros_y;
g_data.tmp_z += gyros_z;
delay(10);
}
a_data.ax = a_data.tmp_x/n;
a_data.ay = a_data.tmp_y/n;
a_data.az = a_data.tmp_z/n;
g_data.gx = g_data.tmp_x/n;
g_data.gy = g_data.tmp_y/n;
g_data.gz = g_data.tmp_z/n;
}
else Serial.print("MPU non identified");
}
void MPU6050_obj::data_processing(const accel_data& a_data ,const gyros_data& g_data, accel_gyros& data){
int s_a = 16384, s_g = 131; //accelerometer/gyroscope sensitivity
float alpha = 0.98;//weight factor
float a_roll = 0, a_pitch = 0, g_roll = 0, g_pitch = 0, yaw = 0;
unsigned long currentTime = micros();
float dt = (currentTime - previousTime) / 1000000.0; // Calculate dt in seconds
int16_t accel_x, accel_y, accel_z, gyros_x, gyros_y, gyros_z;
previousTime = currentTime; //PreviousTime is defined as static
//Start measurements
mpu_lib.getAcceleration(&accel_x, &accel_y, &accel_z);
mpu_lib.getRotation(&gyros_x, &gyros_y, &gyros_z);
//Subtracting the bias (calculated in calibration function) and dividing by sensitivy in order to convert raw values to dps and gravitation force (G)
data.ax = (static_cast<float>(accel_x) - static_cast<float>(a_data.ax))/s_a;
data.ay = (static_cast<float>(accel_y) - static_cast<float>(a_data.ay))/s_a;
data.az = (static_cast<float>(accel_z) - static_cast<float>(a_data.az))/s_a;
data.gx = ((static_cast<float>(gyros_x) - static_cast<float>(g_data.gx))/s_g) * (PI / 180.0); //rad/s
data.gy = ((static_cast<float>(gyros_y) - static_cast<float>(g_data.gy))/s_g) * (PI / 180.0); //rad/s
data.gz = ((static_cast<float>(gyros_z) - static_cast<float>(g_data.gz))/s_g) * (PI / 180.0);
//Calculating roll and pitch from accel readings
a_roll = atan2(data.ay, sqrt(pow(data.ax, 2) + pow(data.az, 2)));
a_pitch = atan2(-data.ax, sqrt(pow(data.ay, 2) + pow(data.az, 2)));
//Integrate gyroscope data for angular velocity
yaw += data.gz * dt;
g_roll += data.gx * dt;
g_pitch += data.gy * dt;
// Complementary filter
data.roll = alpha * g_roll + (1 - alpha) * a_roll;
data.pitch = alpha * g_pitch + (1 - alpha) * a_pitch;
data.yaw = yaw;
}