MPU9250 9DOF e filtro Madgwick

La MPU9250 e' un dispositivo con accelerometro, giroscopio e magnetometro triassiale

Per interfacciare l'MPU9250 ho usato la libreria https://github.com/sparkfun/MPU-9250_Breakout che funziona solo con schede Arduino con processore SAM (quindi MKR)

Nello sketch degli esempi e' stato aggiunto il filtro Madgwick

5852 dati

Pitch (filtrato)
media 60.475°
std 0.22°
skew -0.597

Roll (filtrato)
media 50.471°
std 0.34°
skew -0.113

Yaw (filtrato)
media 209.56°
std 0.54°
skew 0.2

---------------------------------------------------------------------------
#include <SparkFunMPU9250-DMP.h>
#include  <MadgwickAHRS.h> 



MPU9250_DMP imu;
Madgwick  MadgwickFilter;


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

   MadgwickFilter.begin(100);  // 100 Hz

  if (imu.begin() != INV_SUCCESS)
  {
    while (1)
    {
      Serial.println("Unable to communicate with MPU-9250");
      Serial.println("Check connections, and try again.");
      Serial.println();
      delay(5000);
    }
  }


  imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);


  imu.setGyroFSR(2000); // Set gyro to 2000 dps
  // Accel options are +/- 2, 4, 8, or 16 g
  imu.setAccelFSR(2); // Set accel to +/-2g
  // Note: the MPU-9250's magnetometer FSR is set at 
  // +/- 4912 uT (micro-tesla's)

  // setLPF() can be used to set the digital low-pass filter
  // of the accelerometer and gyroscope.
  // Can be any of the following: 188, 98, 42, 20, 10, 5
  // (values are in Hz).
  imu.setLPF(5); // Set LPF corner frequency to 5Hz

  // The sample rate of the accel/gyro can be set using
  // setSampleRate. Acceptable values range from 4Hz to 1kHz
  imu.setSampleRate(10); // Set sample rate to 10Hz

  // Likewise, the compass (magnetometer) sample rate can be
  // set using the setCompassSampleRate() function.
  // This value can range between: 1-100Hz
  imu.setCompassSampleRate(10); // Set mag rate to 10Hz

  imu.dmpBegin(DMP_FEATURE_GYRO_CAL |   // Enable gyro cal
              DMP_FEATURE_SEND_CAL_GYRO,// Send cal'd gyro values
              10);         
}

void loop() 
{

  if ( imu.dataReady() )
  {
    
    imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS);
    printIMUData();
  }
}

void printIMUData(void)
{  

  float accelX = imu.calcAccel(imu.ax);
  float accelY = imu.calcAccel(imu.ay);
  float accelZ = imu.calcAccel(imu.az);
  float gyroX = imu.calcGyro(imu.gx);
  float gyroY = imu.calcGyro(imu.gy);
  float gyroZ = imu.calcGyro(imu.gz);
  float magX = imu.calcMag(imu.mx);
  float magY = imu.calcMag(imu.my);
  float magZ = imu.calcMag(imu.mz);

  MadgwickFilter.update(accelX,accelY,accelZ,gyroX,gyroY,gyroZ,magX,magY,magZ) ;

  
  Serial.print(String(accelX) + "," +String(accelY) + "," + String(accelZ));
  Serial.print("," + String(gyroX) + "," + String(gyroY) + "," + String(gyroZ) + ",");
  Serial.print(String(magX) + "," + String(magY) + "," + String(magZ)+ "," );
  Serial.print(String(MadgwickFilter.getPitch())+ "," +String(MadgwickFilter.getRoll())+ "," +String(MadgwickFilter.getYaw()));
  Serial.println();
}