Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

DCM.cpp@1:da3b20b5d38a, 2012-11-08 (annotated)

Committer:
tylerjw
Date:
Thu Nov 08 20:13:35 2012 +0000
Revision:
1:da3b20b5d38a
Parent:
0:014ee3239c80 
Modified AHRS library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 0:014ee3239c80 1 /* This file is part of the Razor AHRS Firmware */
tylerjw 0:014ee3239c80 2 #include "AHRS.h"
tylerjw 0:014ee3239c80 3 #include <math.h>
tylerjw 0:014ee3239c80 4
tylerjw 0:014ee3239c80 5 // DCM algorithm
tylerjw 0:014ee3239c80 6
tylerjw 0:014ee3239c80 7 /**************************************************/
tylerjw 0:014ee3239c80 8 void IMU::Normalize(void)
tylerjw 0:014ee3239c80 9 {
tylerjw 0:014ee3239c80 10 float error=0;
tylerjw 0:014ee3239c80 11 float temporary[3][3];
tylerjw 0:014ee3239c80 12 float renorm=0;
tylerjw 0:014ee3239c80 13
tylerjw 0:014ee3239c80 14 error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
tylerjw 0:014ee3239c80 15
tylerjw 0:014ee3239c80 16 Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
tylerjw 0:014ee3239c80 17 Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
tylerjw 0:014ee3239c80 18
tylerjw 0:014ee3239c80 19 Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
tylerjw 0:014ee3239c80 20 Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
tylerjw 0:014ee3239c80 21
tylerjw 0:014ee3239c80 22 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
tylerjw 0:014ee3239c80 23
tylerjw 0:014ee3239c80 24 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
tylerjw 0:014ee3239c80 25 Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
tylerjw 0:014ee3239c80 26
tylerjw 0:014ee3239c80 27 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
tylerjw 0:014ee3239c80 28 Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
tylerjw 0:014ee3239c80 29
tylerjw 0:014ee3239c80 30 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
tylerjw 0:014ee3239c80 31 Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
tylerjw 0:014ee3239c80 32 }
tylerjw 0:014ee3239c80 33
tylerjw 0:014ee3239c80 34 /**************************************************/
tylerjw 0:014ee3239c80 35 void IMU::Drift_correction(void)
tylerjw 0:014ee3239c80 36 {
tylerjw 0:014ee3239c80 37 float mag_heading_x;
tylerjw 0:014ee3239c80 38 float mag_heading_y;
tylerjw 0:014ee3239c80 39 float errorCourse;
tylerjw 0:014ee3239c80 40 //Compensation the Roll, Pitch and Yaw drift.
tylerjw 0:014ee3239c80 41 static float Scaled_Omega_P[3];
tylerjw 0:014ee3239c80 42 static float Scaled_Omega_I[3];
tylerjw 0:014ee3239c80 43 float Accel_magnitude;
tylerjw 0:014ee3239c80 44 float Accel_weight;
tylerjw 0:014ee3239c80 45
tylerjw 0:014ee3239c80 46
tylerjw 0:014ee3239c80 47 //*****Roll and Pitch***************
tylerjw 0:014ee3239c80 48
tylerjw 0:014ee3239c80 49 // Calculate the magnitude of the accelerometer vector
tylerjw 0:014ee3239c80 50 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
tylerjw 0:014ee3239c80 51 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
tylerjw 0:014ee3239c80 52 // Dynamic weighting of accelerometer info (reliability filter)
tylerjw 0:014ee3239c80 53 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
tylerjw 0:014ee3239c80 54 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
tylerjw 0:014ee3239c80 55
tylerjw 0:014ee3239c80 56 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
tylerjw 0:014ee3239c80 57 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
tylerjw 0:014ee3239c80 58
tylerjw 0:014ee3239c80 59 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
tylerjw 0:014ee3239c80 60 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
tylerjw 0:014ee3239c80 61
tylerjw 0:014ee3239c80 62 //*****YAW***************
tylerjw 0:014ee3239c80 63 // We make the gyro YAW drift correction based on compass magnetic heading
tylerjw 0:014ee3239c80 64
tylerjw 0:014ee3239c80 65 mag_heading_x = cos(MAG_Heading);
tylerjw 0:014ee3239c80 66 mag_heading_y = sin(MAG_Heading);
tylerjw 0:014ee3239c80 67 errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
tylerjw 0:014ee3239c80 68 Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
tylerjw 0:014ee3239c80 69
tylerjw 0:014ee3239c80 70 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
tylerjw 0:014ee3239c80 71 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
tylerjw 0:014ee3239c80 72
tylerjw 0:014ee3239c80 73 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
tylerjw 0:014ee3239c80 74 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
tylerjw 0:014ee3239c80 75 }
tylerjw 0:014ee3239c80 76
tylerjw 0:014ee3239c80 77 void IMU::Matrix_update(void)
tylerjw 0:014ee3239c80 78 {
tylerjw 0:014ee3239c80 79 Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
tylerjw 0:014ee3239c80 80 Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
tylerjw 0:014ee3239c80 81 Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
tylerjw 0:014ee3239c80 82
tylerjw 0:014ee3239c80 83 Accel_Vector[0]=accel[0];
tylerjw 0:014ee3239c80 84 Accel_Vector[1]=accel[1];
tylerjw 0:014ee3239c80 85 Accel_Vector[2]=accel[2];
tylerjw 0:014ee3239c80 86
tylerjw 0:014ee3239c80 87 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
tylerjw 0:014ee3239c80 88 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
tylerjw 0:014ee3239c80 89
tylerjw 0:014ee3239c80 90 #if DEBUG__NO_DRIFT_CORRECTION == true // Do not use drift correction
tylerjw 0:014ee3239c80 91 Update_Matrix[0][0]=0;
tylerjw 0:014ee3239c80 92 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
tylerjw 0:014ee3239c80 93 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
tylerjw 0:014ee3239c80 94 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
tylerjw 0:014ee3239c80 95 Update_Matrix[1][1]=0;
tylerjw 0:014ee3239c80 96 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
tylerjw 0:014ee3239c80 97 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
tylerjw 0:014ee3239c80 98 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
tylerjw 0:014ee3239c80 99 Update_Matrix[2][2]=0;
tylerjw 0:014ee3239c80 100 #else // Use drift correction
tylerjw 0:014ee3239c80 101 Update_Matrix[0][0]=0;
tylerjw 0:014ee3239c80 102 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
tylerjw 0:014ee3239c80 103 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
tylerjw 0:014ee3239c80 104 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
tylerjw 0:014ee3239c80 105 Update_Matrix[1][1]=0;
tylerjw 0:014ee3239c80 106 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
tylerjw 0:014ee3239c80 107 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
tylerjw 0:014ee3239c80 108 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
tylerjw 0:014ee3239c80 109 Update_Matrix[2][2]=0;
tylerjw 0:014ee3239c80 110 #endif
tylerjw 0:014ee3239c80 111
tylerjw 0:014ee3239c80 112 Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
tylerjw 0:014ee3239c80 113
tylerjw 0:014ee3239c80 114 for(int x=0; x<3; x++) //Matrix Addition (update)
tylerjw 0:014ee3239c80 115 {
tylerjw 0:014ee3239c80 116 for(int y=0; y<3; y++)
tylerjw 0:014ee3239c80 117 {
tylerjw 0:014ee3239c80 118 DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
tylerjw 0:014ee3239c80 119 }
tylerjw 0:014ee3239c80 120 }
tylerjw 0:014ee3239c80 121 }
tylerjw 0:014ee3239c80 122
tylerjw 0:014ee3239c80 123 void IMU::Euler_angles(void)
tylerjw 0:014ee3239c80 124 {
tylerjw 0:014ee3239c80 125 pitch = -asin(DCM_Matrix[2][0]);
tylerjw 0:014ee3239c80 126 roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
tylerjw 0:014ee3239c80 127 yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
tylerjw 0:014ee3239c80 128 }
tylerjw 0:014ee3239c80 129