MPU6050 - Problems with the gyroscope

Hi everyone!
I'm trying to get the angle of inclination of the MPU6050 by using a complementary filter. The angle gotten by the accelerometer works well but I can't get any angle by the gyroscope, it always prints 0, making the filter useless.
I'm using an ESP32, a MPU6050 and the library Adafruit_MPU6050.
Thank you all and sorry for my bad English.

Edit: The gy variable seems to print valours between 4 and -4. So I think it can not be broken.
Here is the code:

#include <Arduino.h>
#include <Wire.h>
#include  <Adafruit_MPU6050.h>
#include <Math.h>

Adafruit_MPU6050 mpu;
Adafruit_Sensor *mpu_accel, *mpu_gyro;


bool start = 0;
float accelOffsetX = 0.0;
float accelOffsetY = 0.0;
float accelOffsetZ = 0.0;

float gyroOffsetX = 0.0;
float gyroOffsetY = 0.0;
float gyroOffsetZ = 0.0;

float ax,ay,az;
float gx,gy,gz;

long time_prev;
float dt;
float ang_x, ang_y;
float ang_x_prev, ang_y_prev;
float accel_ang_y;


float girosc_ang_y_prev; 
float girosc_ang_y;

void lecture(void){
  sensors_event_t accel;
  sensors_event_t gyro;

  mpu_accel->getEvent(&accel);
  mpu_gyro->getEvent(&gyro);
  //////////////////////////////////////////OFFSET/////////////////////////
  ax=accel.acceleration.x - accelOffsetX;
  ay=accel.acceleration.y - accelOffsetY;
  az=accel.acceleration.z + accelOffsetZ;

  gx=(gyro.gyro.x - gyroOffsetX);
  gy=(gyro.gyro.y -  gyroOffsetY);
  gz=(gyro.gyro.z - gyroOffsetZ);

}

void calibrate(void) {
  // Read raw data and calculate offsets
  sensors_event_t accel;
  sensors_event_t gyro;
  sensors_event_t temp;

  
  for (int i = 0; i < 1000; i++) {
  
    mpu_accel->getEvent(&accel);
    mpu_gyro->getEvent(&gyro);

    accelOffsetX += accel.acceleration.x;
    accelOffsetY += accel.acceleration.y;
    accelOffsetZ += accel.acceleration.z;

    gyroOffsetX += gyro.gyro.x;
    gyroOffsetY += gyro.gyro.y;
    gyroOffsetZ += gyro.gyro.z;

    delay(10);
  }

  // Calculate average offsets
  accelOffsetX /= 1000.0;
  accelOffsetY /= 1000.0;
  accelOffsetZ /= 1000.0;
  accelOffsetZ = 9.8 - accelOffsetZ;

  gyroOffsetX /= 1000.0;
  gyroOffsetY /= 1000.0;
  gyroOffsetZ /= 1000.0;
}


void setup(void) {
  Serial.begin(9600);
  

  if (!mpu.begin()) {
    //Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
  delay(500);

  mpu_accel = mpu.getAccelerometerSensor();
  mpu_accel->printSensorDetails();

  mpu_gyro = mpu.getGyroSensor();
  mpu_gyro->printSensorDetails();
  delay(100);
  calibrate();
}

void loop() {
  //////////////////////////////////////////Lecture ////////////////////////////
  lecture();
  
///////////////////////////////////// Complementary filter////////////////////////////////////////////

  dt = (millis() - time_prev) / 1000.0;
  time_prev = millis();

  //Accelerometer angle calculation
  accel_ang_y = atan(-ax / sqrt(pow(ay, 2) + pow(az, 2)))*(180.0 / 3.14);
  
  if(start == 0){
    ang_y_prev = accel_ang_y;
    girosc_ang_y_prev = 0;  
    start = 1;
  }

  //complementary filter
  ang_y = 0.98*(ang_y_prev + (gy/131.0)*dt) + 0.02*accel_ang_y;

  // Accumulate the gyro rate to get the total gyro angle
  girosc_ang_y = girosc_ang_y_prev + (gy/131.0)*dt;

  ang_y_prev = ang_y;
  girosc_ang_y_prev = girosc_ang_y;

  ///////////////////////////////prints//////////////////////////////
  Serial.print(">Complementary:");
  Serial.println(ang_y);
  Serial.print(">Giroscopy:");
  Serial.println(girosc_ang_y);
  Serial.print(">Acelerometer:");
  Serial.println(accel_ang_y);
 
}

1 post - 1 participant

Read full topic