Filtering mpu9250 data using madgwick filter

Other Hardware Sensors
1

Hello there,

I bought mpu9250 IMU sensor to control servo motor
and i want to know how to filter its data using madgwick filter?

I'm using arduino nano, arduino IDE v1.13.8

I found a library for the filter but it seems to work only for arduino 101.

I need a library or code that I can use to filter accelerometer, gyroscope, and magnetometer data.

where can I find one? i searched on google like crazy but didn't find anything useful.

here's the code that I'm using...

#include "MPU9250.h"
#include <Servo.h>

// an MPU9250 object with the MPU-9250 sensor on I2C bus 0 with address 0x68
MPU9250 IMU(Wire,0x68);
Servo myServo;
int status;
double roll , pitch, yaw;
float Xh;
float Yh;

int servo_pin = 5;
void setup() {
  // serial to display data
  Serial.begin(115200);
  while(!Serial) {}

  // start communication with IMU 
  status = IMU.begin();
  if (status < 0) {
    Serial.println("IMU initialization unsuccessful");
    Serial.println("Check IMU wiring or try cycling power");
    Serial.print("Status: ");
    Serial.println(status);
    while(1) {}
  }
  // setting the accelerometer full scale range to +/-8G 
  IMU.setAccelRange(MPU9250::ACCEL_RANGE_8G);
  // setting the gyroscope full scale range to +/-500 deg/s
  IMU.setGyroRange(MPU9250::GYRO_RANGE_500DPS);
  // setting DLPF bandwidth to 20 Hz
  IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
  // setting SRD to 19 for a 50 Hz update rate
  IMU.setSrd(19);
  
  myServo.attach(servo_pin);
}

void loop() {
  
  // read the sensor
  IMU.readSensor();
/*
  // display the data
  Serial.print(IMU.getAccelX_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getAccelY_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getAccelZ_mss(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroX_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroY_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getGyroZ_rads(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagX_uT(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagY_uT(),6);
  Serial.print("\t");
  Serial.print(IMU.getMagZ_uT(),6);
  Serial.print("\t");
  Serial.println(IMU.getTemperature_C(),6);
*/
pitch = atan2 (IMU.getAccelY_mss() ,( sqrt ((IMU.getAccelX_mss() * IMU.getAccelX_mss()) + (IMU.getAccelZ_mss() * IMU.getAccelZ_mss()))));
roll  = atan2(-IMU.getAccelX_mss() ,( sqrt((IMU.getAccelY_mss() * IMU.getAccelY_mss()) + (IMU.getAccelZ_mss() * IMU.getAccelZ_mss()))));

Yh = (IMU.getMagY_uT() * cos(roll)) - (IMU.getMagZ_uT() * sin(roll));
Xh = (IMU.getMagX_uT() * cos(pitch))+(IMU.getMagY_uT() * sin(roll)*sin(pitch)) + (IMU.getMagZ_uT() * cos(roll) * sin(pitch));

yaw   =  atan2(Yh, Xh);


roll  = roll*57.3;
pitch = pitch*57.3;
yaw   = yaw*57.3;




Serial.print("pitch: ");
Serial.print(pitch);
Serial.print("  ");
Serial.print("roll: ");  
Serial.println(roll);
Serial.print("  ");

Serial.print("yaw: ");  
Serial.println(yaw);
Serial.print("  ");


delay(100);
myServo.write(yaw);
}
2

This one works, if you make the effort to calibrate the sensor correctly.

The MPU-9250 Madgwick/Mahony filters posted by Sparkfun and Kris Winer don't work.

Keep in mind that the MPU-6050 and MPU-9250 are obsolete and have not been manufactured for some time, so any cheap modules that you buy from Amazon, Alibaba, etc. probably use reject or counterfeit chips.

3

thank you so much
i will give it a try right now

4

i cant add the library
it gives this error "Invalid library found in C:\Users\HPP\Documents\Arduino\libraries\MPU-9250-AHRS: no headers files (.h) found in C:\Users\HPP\Documents\Arduino\libraries\MPU-9250-AHRS"

i downloaded the library manually since I cant even add it as a zip file

5

Those libraries are not intended to be installed normally. They should be put in a /lib folder in the folder for the MPU-9250 AHRS sketch.

1 Like
6

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.