giovedì 28 marzo 2019

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();
}

Nessun commento:

Posta un commento

Arduino logger low power

Alla fine ci sono riuscito La arduino MKR zero e' ormai da un mese che e' rimasta accesa sul tavolo...o meglio si attiva ogni minuto...