Created
June 5, 2017 20:37
-
-
Save shoebahmedadeel/942e230086bafa83da7cc59624ee8657 to your computer and use it in GitHub Desktop.
Mpu 9250 complimentary filter to get roll pitch and stabilization
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include <Wire.h> | |
#include <Servo.h> | |
#define SerialPort Serial | |
#include <SparkFunMPU9250-DMP.h> | |
MPU9250_DMP imu; | |
double roll , pitch, yaw; | |
float phi = 0; | |
float theta = 0; | |
float psi = 0; | |
long int pre_ts=0; | |
float phi_n_1, theta_n_1, psi_n_1; | |
float phi_dot, theta_dot, psi_dot, phi_quat; | |
long gyro_x_cal, gyro_y_cal, gyro_z_cal; | |
float Q[4] = {1,0,0,0} ; | |
float Q_dot [4] ; | |
float Q_pre [4] = {1,0,0,0} ; | |
//Servo intialization | |
Servo Servo1; | |
Servo Servo2; | |
//Servo Servo3; | |
int roll_servo; | |
int pitch_servo; | |
//int yaw_servo; | |
//feedback intialization | |
int feedbackPin = A0; | |
int feedback; | |
float voltage; | |
//RC intialization | |
const int chA = 8; | |
const int chB = 9; | |
const int chC = 10; | |
const int chD = 11; | |
int ch1; | |
int ch2; | |
int ch3; | |
int ch4; | |
void setup() | |
{ | |
Servo1.attach(5); | |
Servo2.attach(6); | |
{ | |
{ | |
SerialPort.begin(115200); | |
if (imu.begin() != INV_SUCCESS) | |
{ | |
while (1) | |
{ | |
SerialPort.println("Unable to communicate with MPU-9250"); | |
SerialPort.println("Check connections, and try again."); | |
SerialPort.println(); | |
delay(3000); | |
} | |
} | |
imu.setSensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS); | |
imu.setGyroFSR(250); // Set gyro to 2000 dps | |
// Accel options are +/- 2, 4, 8, or 16 g | |
imu.setAccelFSR(2); // Set accel to +/-2g | |
imu.setLPF(10); // Set LPF corner frequency to 5Hz | |
imu.setSampleRate(10); // Set sample rate to 10Hz | |
imu.setCompassSampleRate(50); // Set mag rate to 10Hz | |
} | |
//Wire.begin(); | |
//for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ | |
//read_IMUDATA(); | |
pre_ts=millis(); | |
} | |
} | |
void loop() | |
{ | |
if ( imu.dataReady() ) | |
{ | |
imu.update(UPDATE_ACCEL | UPDATE_GYRO | UPDATE_COMPASS); | |
printIMUData(millis()-pre_ts); | |
pre_ts=millis(); | |
} | |
} | |
void printIMUData(long int dt) | |
{ | |
float accelX = imu.calcAccel(imu.ax); | |
float accelY = imu.calcAccel(imu.ay); | |
float accelZ = imu.calcAccel(imu.az); | |
float gyroX = imu.calcGyro(imu.gx)/57.3; | |
float gyroY = imu.calcGyro(imu.gy)/57.3; | |
float gyroZ = imu.calcGyro(imu.gz)/57.3; | |
float magX = imu.calcMag(imu.mx); | |
float magY = imu.calcMag(imu.my); | |
float magZ = imu.calcMag(imu.mz); | |
// NORMALIZE ACCELE VALUES | |
float naccel = sqrt(accelX*accelX +accelY*accelY+accelZ*accelZ); | |
accelX =accelX/naccel; | |
accelY =accelY/naccel; | |
accelZ =accelZ/naccel; | |
//Normailze mag values | |
float nmag = sqrt(magX*magX + magY*magY + magZ*magZ); | |
magX =magX/ nmag; | |
magY =magY/ nmag; | |
magZ =magZ/ nmag; | |
//Euler angle from accel | |
pitch = atan2 (accelY ,( sqrt ((accelX * accelX) + (accelZ * accelZ)))); | |
roll = atan2(-accelX ,( sqrt((accelY * accelY) + (accelZ * accelZ)))); | |
// yaw from mag | |
float Yh = (magY * cos(roll)) + (magZ * sin(roll)); | |
float Xh = (magX * cos(pitch))+(magY * sin(roll)*sin(pitch)) + (magZ * cos(roll) * sin(pitch)); | |
yaw = atan2(Yh, Xh); | |
roll = (0.98*(roll+(gyroX)*dt/1000.0f) +0.02*(accelX))*57.3; | |
pitch = (0.98*(pitch + gyroY*dt/1000.0f)+0.02*(accelY))*57.3; | |
yaw =((yaw+ gyroZ*dt/1000.0f)+0.02*(magZ))*57.3; | |
// quaternions | |
Q_dot[0] = -0.5* ( (gyroX*Q_pre[1]) +(gyroY*Q_pre[2]) + (Q_pre[3]*gyroZ)); | |
Q_dot[1] = 0.5* ( (gyroX*Q_pre[0]) +(gyroZ*Q_pre[2]) - (Q_pre[3]*gyroY)); | |
Q_dot[2] = 0.5* ( (gyroY*Q_pre[0]) -(gyroZ*Q_pre[1]) + (Q_pre[3]*gyroX)); | |
Q_dot[3] = 0.5* ( (gyroZ*Q_pre[0]) +(gyroY*Q_pre[1]) - (Q_pre[2]*gyroX)); | |
Q[0] =Q_pre[0]+ (Q_dot[0]*dt/1000.0); | |
Q_pre[0] = Q[0]; | |
Q[1] =Q_pre[1]+ (Q_dot[1]*dt/1000.0); | |
Q_pre[1] = Q[1]; | |
Q[2] =Q_pre[2]+ (Q_dot[2]*dt/1000.0); | |
Q_pre[2] = Q[2]; | |
Q[3] =Q_pre[3]+ (Q_dot[3]*dt/1000.0); | |
Q_pre[3] = Q[3]; | |
double n = (sqrt((Q[0]*Q[0]) + (Q[1]*Q[1]) + (Q[2]*Q[2]) + (Q[3]*Q[3]))); | |
float Q0 =Q[0] / n; | |
float Q1 =Q[1] / n; | |
float Q2 = Q[2]/n; | |
float Q3 = Q[3]/n; | |
phi_quat = atan2 (2*((Q0*Q1)+(Q2*Q3)), ((0.5f-Q1*Q1-Q2*Q2))); | |
float theta_quat = asin( 2*((Q0*Q2)- (Q1*Q3))); | |
float psi_quat = atan2(2*((Q0*Q3)+(Q1*Q2)),((0.5f-Q2*Q2+Q3*Q3))); | |
phi_quat = (0.98*(phi_quat+gyroX*dt/1000.0f) +0.02*(accelX))*57.3; | |
theta_quat = (0.98*(theta_quat+gyroY*dt/1000.0f) +0.02*(accelY))*57.3; | |
psi_quat = (0.98*(psi_quat+gyroZ*dt/1000.0f) +0.02*(magZ))*57.3; | |
int filtered_roll = 0.99*(roll+roll*dt/1000.0f)+0.01*(phi_quat); | |
int filtered_pitch = 0.99*(pitch+pitch*dt/1000.0f)+0.01*(theta_quat); | |
int filtered_yaw = 0.5*(yaw+yaw*dt/1000.0f)+0.5*(psi_quat); | |
Serial.println(String(filtered_roll)+','+String(filtered_pitch)+','+String(filtered_yaw)); | |
//switch the stabilize mode and control mode | |
ch4 = pulseIn(11, HIGH, 25000); | |
if | |
(ch4 <= 1500) | |
{ | |
// stabilize mode | |
filtered_roll = map(filtered_roll, -90, 90, 0, 180); | |
filtered_roll = constrain(filtered_roll, 0,180 ); | |
Servo1.write(filtered_roll ); | |
filtered_pitch = map(filtered_pitch, -90, 90, 0, 180); | |
filtered_pitch = constrain(filtered_pitch, 0,180 ); | |
Servo2.write(filtered_pitch); | |
} | |
// control mode | |
else | |
{ | |
{ | |
filtered_roll = map(filtered_roll, -90, 90, 0, 180); | |
filtered_roll = constrain(filtered_roll, 0,180 ); | |
Servo1.write(filtered_roll ); | |
filtered_pitch = map(filtered_pitch, -90, 90, 0, 180); | |
filtered_pitch = constrain(filtered_pitch, 0,180 ); | |
Servo2.write(filtered_pitch ); | |
} | |
ch1 = pulseIn (8, HIGH,25000); | |
ch1 = map(ch1, 1000,2000,-90,90); | |
Servo1.write(ch1+ filtered_roll); | |
ch2 = pulseIn (9, HIGH,25000); | |
ch2 = map(ch2, 1000,2000,-90,90); | |
Servo2.write(ch2 + filtered_pitch); | |
} | |
} | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment