Prosper Van / DCM_AHRS_GY80

AHRS based on MatrixPilot DCM algorithm; ported from Pololu MinIMU-9 example code in turn based on ArduPilot 1.5 built for sensor gy_80

Dependents:   quadCommand2

Fork of DCM_AHRS by Michael Shimniok

DCM.cpp@0:62284d27d75e, 2012-01-24 (annotated)

Committer:
shimniok
Date:
Tue Jan 24 17:40:40 2012 +0000
Revision:
0:62284d27d75e
Child:
1:115cf0a84a9d
Initial version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:62284d27d75e 1 /*
shimniok 0:62284d27d75e 2 MinIMU-9-mbed-AHRS
shimniok 0:62284d27d75e 3 Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)
shimniok 0:62284d27d75e 4
shimniok 0:62284d27d75e 5 Modified and ported to mbed environment by Michael Shimniok
shimniok 0:62284d27d75e 6 http://www.bot-thoughts.com/
shimniok 0:62284d27d75e 7
shimniok 0:62284d27d75e 8 Basedd on MinIMU-9-Arduino-AHRS
shimniok 0:62284d27d75e 9 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
shimniok 0:62284d27d75e 10
shimniok 0:62284d27d75e 11 Copyright (c) 2011 Pololu Corporation.
shimniok 0:62284d27d75e 12 http://www.pololu.com/
shimniok 0:62284d27d75e 13
shimniok 0:62284d27d75e 14 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
shimniok 0:62284d27d75e 15 http://code.google.com/p/sf9domahrs/
shimniok 0:62284d27d75e 16
shimniok 0:62284d27d75e 17 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
shimniok 0:62284d27d75e 18 Julio and Doug Weibel:
shimniok 0:62284d27d75e 19 http://code.google.com/p/ardu-imu/
shimniok 0:62284d27d75e 20
shimniok 0:62284d27d75e 21 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
shimniok 0:62284d27d75e 22 under the terms of the GNU Lesser General Public License as published by the
shimniok 0:62284d27d75e 23 Free Software Foundation, either version 3 of the License, or (at your option)
shimniok 0:62284d27d75e 24 any later version.
shimniok 0:62284d27d75e 25
shimniok 0:62284d27d75e 26 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
shimniok 0:62284d27d75e 27 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
shimniok 0:62284d27d75e 28 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
shimniok 0:62284d27d75e 29 more details.
shimniok 0:62284d27d75e 30
shimniok 0:62284d27d75e 31 You should have received a copy of the GNU Lesser General Public License along
shimniok 0:62284d27d75e 32 with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
shimniok 0:62284d27d75e 33
shimniok 0:62284d27d75e 34 */
shimniok 0:62284d27d75e 35 #include "DCM.h"
shimniok 0:62284d27d75e 36 #include "Matrix.h"
shimniok 0:62284d27d75e 37 #include "math.h"
shimniok 0:62284d27d75e 38 #include "Sensors.h"
shimniok 0:62284d27d75e 39
shimniok 0:62284d27d75e 40 #define MAG_SKIP 2
shimniok 0:62284d27d75e 41
shimniok 0:62284d27d75e 42 float DCM::constrain(float x, float a, float b)
shimniok 0:62284d27d75e 43 {
shimniok 0:62284d27d75e 44 float result = x;
shimniok 0:62284d27d75e 45
shimniok 0:62284d27d75e 46 if (x < a) result = a;
shimniok 0:62284d27d75e 47 if (x > b) result = b;
shimniok 0:62284d27d75e 48
shimniok 0:62284d27d75e 49 return result;
shimniok 0:62284d27d75e 50 }
shimniok 0:62284d27d75e 51
shimniok 0:62284d27d75e 52
shimniok 0:62284d27d75e 53 DCM::DCM(void):
shimniok 0:62284d27d75e 54 G_Dt(0.02), update_count(MAG_SKIP)
shimniok 0:62284d27d75e 55 {
shimniok 0:62284d27d75e 56 for (int m=0; m < 3; m++) {
shimniok 0:62284d27d75e 57 Accel_Vector[m] = 0;
shimniok 0:62284d27d75e 58 Gyro_Vector[m] = 0;
shimniok 0:62284d27d75e 59 Omega_Vector[m] = 0;
shimniok 0:62284d27d75e 60 Omega_P[m] = 0;
shimniok 0:62284d27d75e 61 Omega_I[m] = 0;
shimniok 0:62284d27d75e 62 Omega[m] = 0;
shimniok 0:62284d27d75e 63 errorRollPitch[m] = 0;
shimniok 0:62284d27d75e 64 errorYaw[m] = 0;
shimniok 0:62284d27d75e 65 for (int n=0; n < 3; n++) {
shimniok 0:62284d27d75e 66 dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
shimniok 0:62284d27d75e 67 }
shimniok 0:62284d27d75e 68 }
shimniok 0:62284d27d75e 69
shimniok 0:62284d27d75e 70 }
shimniok 0:62284d27d75e 71
shimniok 0:62284d27d75e 72
shimniok 0:62284d27d75e 73 /**************************************************/
shimniok 0:62284d27d75e 74 void DCM::Normalize(void)
shimniok 0:62284d27d75e 75 {
shimniok 0:62284d27d75e 76 float error=0;
shimniok 0:62284d27d75e 77 float temporary[3][3];
shimniok 0:62284d27d75e 78 float renorm=0;
shimniok 0:62284d27d75e 79
shimniok 0:62284d27d75e 80 error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
shimniok 0:62284d27d75e 81
shimniok 0:62284d27d75e 82 Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
shimniok 0:62284d27d75e 83 Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
shimniok 0:62284d27d75e 84
shimniok 0:62284d27d75e 85 Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
shimniok 0:62284d27d75e 86 Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
shimniok 0:62284d27d75e 87
shimniok 0:62284d27d75e 88 Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
shimniok 0:62284d27d75e 89
shimniok 0:62284d27d75e 90 renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
shimniok 0:62284d27d75e 91 Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
shimniok 0:62284d27d75e 92
shimniok 0:62284d27d75e 93 renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
shimniok 0:62284d27d75e 94 Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
shimniok 0:62284d27d75e 95
shimniok 0:62284d27d75e 96 renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
shimniok 0:62284d27d75e 97 Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
shimniok 0:62284d27d75e 98 }
shimniok 0:62284d27d75e 99
shimniok 0:62284d27d75e 100 /**************************************************/
shimniok 0:62284d27d75e 101 void DCM::Drift_correction(void)
shimniok 0:62284d27d75e 102 {
shimniok 0:62284d27d75e 103 float mag_heading_x;
shimniok 0:62284d27d75e 104 float mag_heading_y;
shimniok 0:62284d27d75e 105 float errorCourse;
shimniok 0:62284d27d75e 106 //Compensation the Roll, Pitch and Yaw drift.
shimniok 0:62284d27d75e 107 static float Scaled_Omega_P[3];
shimniok 0:62284d27d75e 108 static float Scaled_Omega_I[3];
shimniok 0:62284d27d75e 109 float Accel_magnitude;
shimniok 0:62284d27d75e 110 float Accel_weight;
shimniok 0:62284d27d75e 111
shimniok 0:62284d27d75e 112
shimniok 0:62284d27d75e 113 //*****Roll and Pitch***************
shimniok 0:62284d27d75e 114
shimniok 0:62284d27d75e 115 // Calculate the magnitude of the accelerometer vector
shimniok 0:62284d27d75e 116 Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
shimniok 0:62284d27d75e 117 Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
shimniok 0:62284d27d75e 118 // Dynamic weighting of accelerometer info (reliability filter)
shimniok 0:62284d27d75e 119 // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
shimniok 0:62284d27d75e 120 Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //
shimniok 0:62284d27d75e 121
shimniok 0:62284d27d75e 122 Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
shimniok 0:62284d27d75e 123 Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
shimniok 0:62284d27d75e 124
shimniok 0:62284d27d75e 125 Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
shimniok 0:62284d27d75e 126 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
shimniok 0:62284d27d75e 127
shimniok 0:62284d27d75e 128 //*****YAW***************
shimniok 0:62284d27d75e 129 // We make the gyro YAW drift correction based on compass magnetic heading
shimniok 0:62284d27d75e 130
shimniok 0:62284d27d75e 131 /* http://tinyurl.com/7bul438
shimniok 0:62284d27d75e 132 * William Premerlani:
shimniok 0:62284d27d75e 133 * 1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees.
shimniok 0:62284d27d75e 134 * A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine
shimniok 0:62284d27d75e 135 * matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with
shimniok 0:62284d27d75e 136 * 90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross
shimniok 0:62284d27d75e 137 * product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector
shimniok 0:62284d27d75e 138 * that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in
shimniok 0:62284d27d75e 139 * MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will
shimniok 0:62284d27d75e 140 * provide a 3 axis lock.
shimniok 0:62284d27d75e 141 * 2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known
shimniok 0:62284d27d75e 142 * as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or
shimniok 0:62284d27d75e 143 * quaternions.
shimniok 0:62284d27d75e 144 *
shimniok 0:62284d27d75e 145 * ummm... we're no actually calculating MAG_Heading anywhere... so it's zero...
shimniok 0:62284d27d75e 146 * mag_earth[3x1] = mag[3x1] dot dcm[3x3]
shimniok 0:62284d27d75e 147 * earth_rotation_error_vector = mag_earth[x and y] cross known_earth_mag[??]
shimniok 0:62284d27d75e 148 * gyro drift error aka body_rotation_error_vector = earth_rotation_error_Vector times dcm[3x3]
shimniok 0:62284d27d75e 149 float mag_earth[3], mag_sensor[3];
shimniok 0:62284d27d75e 150 mag_sensor[0] = magnetom_x;
shimniok 0:62284d27d75e 151 mag_sensor[1] = magnetom_y;
shimniok 0:62284d27d75e 152 mag_sensor[2] = magnetom_z;
shimniok 0:62284d27d75e 153 mag_earth[0] = VectorDotProduct( &dcm[0] , mag_sensor ) << 1;
shimniok 0:62284d27d75e 154 mag_earth[1] = VectorDotProduct( &dcm[1] , mag_sensor ) << 1;
shimniok 0:62284d27d75e 155 mag_earth[2] = VectorDotProduct( &dcm[2] , mag_sensor ) << 1;
shimniok 0:62284d27d75e 156 mag_error = 100 * VectorDotProduct( 2 , mag_earth , declinationVector ) ; // Dotgain = 1/2
shimniok 0:62284d27d75e 157 VectorScale( 3 , errorYawplane , &rmat[6] , mag_error ) ; // Scalegain = 1/2
shimniok 0:62284d27d75e 158 */
shimniok 0:62284d27d75e 159
shimniok 0:62284d27d75e 160 mag_heading_x = cos(MAG_Heading);
shimniok 0:62284d27d75e 161 mag_heading_y = sin(MAG_Heading);
shimniok 0:62284d27d75e 162 errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x); //Calculating YAW error
shimniok 0:62284d27d75e 163 Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
shimniok 0:62284d27d75e 164
shimniok 0:62284d27d75e 165 Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
shimniok 0:62284d27d75e 166 Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
shimniok 0:62284d27d75e 167
shimniok 0:62284d27d75e 168 Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
shimniok 0:62284d27d75e 169 Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
shimniok 0:62284d27d75e 170 }
shimniok 0:62284d27d75e 171
shimniok 0:62284d27d75e 172 /**************************************************/
shimniok 0:62284d27d75e 173 void DCM::Accel_adjust(void)
shimniok 0:62284d27d75e 174 {
shimniok 0:62284d27d75e 175 Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on Acc_y = GPS_speed*GyroZ
shimniok 0:62284d27d75e 176 Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on Acc_z = GPS_speed*GyroY
shimniok 0:62284d27d75e 177 // Add linear (x-axis) acceleration correction here
shimniok 0:62284d27d75e 178
shimniok 0:62284d27d75e 179 // from MatrixPilot
shimniok 0:62284d27d75e 180 // total (3D) airspeed in cm/sec is used to adjust for acceleration
shimniok 0:62284d27d75e 181 //gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , air_speed_3DGPS ) ;
shimniok 0:62284d27d75e 182 //gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , air_speed_3DGPS ) ;
shimniok 0:62284d27d75e 183 //gplane[1]=gplane[1]+ ((unsigned int)(ACCELSCALE))*forward_acceleration
shimniok 0:62284d27d75e 184 }
shimniok 0:62284d27d75e 185
shimniok 0:62284d27d75e 186 /**************************************************/
shimniok 0:62284d27d75e 187 void DCM::Matrix_update(void)
shimniok 0:62284d27d75e 188 {
shimniok 0:62284d27d75e 189 // TODO: Hardware-specific routines to convert gyro to units; this (probably) should be handled elsewhere
shimniok 0:62284d27d75e 190
shimniok 0:62284d27d75e 191 Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
shimniok 0:62284d27d75e 192 Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
shimniok 0:62284d27d75e 193 Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
shimniok 0:62284d27d75e 194
shimniok 0:62284d27d75e 195 // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
shimniok 0:62284d27d75e 196 Accel_Vector[0]=accel_x;
shimniok 0:62284d27d75e 197 Accel_Vector[1]=accel_y;
shimniok 0:62284d27d75e 198 Accel_Vector[2]=accel_z;
shimniok 0:62284d27d75e 199
shimniok 0:62284d27d75e 200 Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
shimniok 0:62284d27d75e 201 Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
shimniok 0:62284d27d75e 202
shimniok 0:62284d27d75e 203 // Remove centrifugal & linear acceleration.
shimniok 0:62284d27d75e 204 Accel_adjust();
shimniok 0:62284d27d75e 205
shimniok 0:62284d27d75e 206 #if OUTPUTMODE==1
shimniok 0:62284d27d75e 207 Update_Matrix[0][0]=0;
shimniok 0:62284d27d75e 208 Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
shimniok 0:62284d27d75e 209 Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
shimniok 0:62284d27d75e 210 Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
shimniok 0:62284d27d75e 211 Update_Matrix[1][1]=0;
shimniok 0:62284d27d75e 212 Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
shimniok 0:62284d27d75e 213 Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
shimniok 0:62284d27d75e 214 Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
shimniok 0:62284d27d75e 215 Update_Matrix[2][2]=0;
shimniok 0:62284d27d75e 216 #else // Uncorrected data (no drift correction)
shimniok 0:62284d27d75e 217 Update_Matrix[0][0]=0;
shimniok 0:62284d27d75e 218 Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
shimniok 0:62284d27d75e 219 Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
shimniok 0:62284d27d75e 220 Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
shimniok 0:62284d27d75e 221 Update_Matrix[1][1]=0;
shimniok 0:62284d27d75e 222 Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
shimniok 0:62284d27d75e 223 Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
shimniok 0:62284d27d75e 224 Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
shimniok 0:62284d27d75e 225 Update_Matrix[2][2]=0;
shimniok 0:62284d27d75e 226 #endif
shimniok 0:62284d27d75e 227
shimniok 0:62284d27d75e 228 Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
shimniok 0:62284d27d75e 229
shimniok 0:62284d27d75e 230 // ??? Matrix_Add(dcm, dcm, Temporary_Matrix); ???
shimniok 0:62284d27d75e 231 for(int x=0; x<3; x++) { //Matrix Addition (update)
shimniok 0:62284d27d75e 232 for(int y=0; y<3; y++) {
shimniok 0:62284d27d75e 233 dcm[x][y] += Temporary_Matrix[x][y];
shimniok 0:62284d27d75e 234 }
shimniok 0:62284d27d75e 235 }
shimniok 0:62284d27d75e 236 }
shimniok 0:62284d27d75e 237
shimniok 0:62284d27d75e 238 void DCM::Euler_angles(void)
shimniok 0:62284d27d75e 239 {
shimniok 0:62284d27d75e 240 pitch = -asin(dcm[2][0]);
shimniok 0:62284d27d75e 241 roll = atan2(dcm[2][1],dcm[2][2]);
shimniok 0:62284d27d75e 242 yaw = atan2(dcm[1][0],dcm[0][0]);
shimniok 0:62284d27d75e 243 }
shimniok 0:62284d27d75e 244
shimniok 0:62284d27d75e 245 void DCM::Update_step()
shimniok 0:62284d27d75e 246 {
shimniok 0:62284d27d75e 247 Read_Gyro();
shimniok 0:62284d27d75e 248 Read_Accel();
shimniok 0:62284d27d75e 249 if (--update_count == 0) {
shimniok 0:62284d27d75e 250 Update_mag();
shimniok 0:62284d27d75e 251 update_count = MAG_SKIP;
shimniok 0:62284d27d75e 252 }
shimniok 0:62284d27d75e 253 Matrix_update();
shimniok 0:62284d27d75e 254 Normalize();
shimniok 0:62284d27d75e 255 Drift_correction();
shimniok 0:62284d27d75e 256 //Accel_adjust();
shimniok 0:62284d27d75e 257 Euler_angles();
shimniok 0:62284d27d75e 258 }
shimniok 0:62284d27d75e 259
shimniok 0:62284d27d75e 260 void DCM::Update_mag()
shimniok 0:62284d27d75e 261 {
shimniok 0:62284d27d75e 262 Read_Compass();
shimniok 0:62284d27d75e 263 Compass_Heading();
shimniok 0:62284d27d75e 264 }