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