Revision 0:62284d27d75e, committed 2012-01-24

Comitter:

shimniok

Date:

Tue Jan 24 17:40:40 2012 +0000

Commit message:

Initial version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM.cpp	Tue Jan 24 17:40:40 2012 +0000
@@ -0,0 +1,264 @@
+/*
+MinIMU-9-mbed-AHRS
+Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)
+
+Modified and ported to mbed environment by Michael Shimniok
+http://www.bot-thoughts.com/
+
+Basedd on MinIMU-9-Arduino-AHRS
+Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
+
+Copyright (c) 2011 Pololu Corporation.
+http://www.pololu.com/
+
+MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
+http://code.google.com/p/sf9domahrs/
+
+sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
+Julio and Doug Weibel:
+http://code.google.com/p/ardu-imu/
+
+MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
+under the terms of the GNU Lesser General Public License as published by the
+Free Software Foundation, either version 3 of the License, or (at your option)
+any later version.
+
+MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
+WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
+more details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
+
+*/
+#include "DCM.h"
+#include "Matrix.h"
+#include "math.h"
+#include "Sensors.h"
+
+#define MAG_SKIP 2
+
+float DCM::constrain(float x, float a, float b) 
+{
+    float result = x;
+    
+    if (x < a) result = a;
+    if (x > b) result = b;
+
+    return result;  
+}
+
+
+DCM::DCM(void):
+    G_Dt(0.02), update_count(MAG_SKIP)
+{
+    for (int m=0; m < 3; m++) {
+        Accel_Vector[m] = 0;
+        Gyro_Vector[m] = 0;
+        Omega_Vector[m] = 0;
+        Omega_P[m] = 0;
+        Omega_I[m] = 0;
+        Omega[m] = 0;
+        errorRollPitch[m] = 0; 
+        errorYaw[m] = 0;
+        for (int n=0; n < 3; n++) {
+            dcm[m][n] = (m == n) ? 1 : 0; // dcm starts as identity matrix
+        }
+    }
+    
+}
+
+
+/**************************************************/
+void DCM::Normalize(void)
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+  
+  error= -Vector_Dot_Product(&dcm[0][0], &dcm[1][0])*.5; //eq.19
+
+  Vector_Scale(&temporary[0][0], &dcm[1][0], error); //eq.19
+  Vector_Scale(&temporary[1][0], &dcm[0][0], error); //eq.19
+  
+  Vector_Add(&temporary[0][0], &temporary[0][0], &dcm[0][0]);//eq.19
+  Vector_Add(&temporary[1][0], &temporary[1][0], &dcm[1][0]);//eq.19
+  
+  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+  Vector_Scale(&dcm[0][0], &temporary[0][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+  Vector_Scale(&dcm[1][0], &temporary[1][0], renorm);
+  
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+  Vector_Scale(&dcm[2][0], &temporary[2][0], renorm);
+}
+
+/**************************************************/
+void DCM::Drift_correction(void)
+{
+  float mag_heading_x;
+  float mag_heading_y;
+  float errorCourse;
+  //Compensation the Roll, Pitch and Yaw drift. 
+  static float Scaled_Omega_P[3];
+  static float Scaled_Omega_I[3];
+  float Accel_magnitude;
+  float Accel_weight;
+  
+  
+  //*****Roll and Pitch***************
+
+  // Calculate the magnitude of the accelerometer vector
+  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+  // Dynamic weighting of accelerometer info (reliability filter)
+  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //  
+
+  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&dcm[2][0]); //adjust the ground of reference
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);     
+  
+  //*****YAW***************
+  // We make the gyro YAW drift correction based on compass magnetic heading
+
+  /* http://tinyurl.com/7bul438
+   * William Premerlani:
+   * 1. If you are treating the magnetometer as a tilt compensated compass, it will not work for pitch values near 90 degrees.
+   *    A better way to do it is to use the magnetometer measurement as a reference vector instead. Use the direction cosine
+   *    matrix to transform the magnetometer vector from body frame to earth frame, which works in any orientation, even with
+   *    90 degree pitch. Then, extract the horizontal component of the magnetometer earth frame vector, and take the cross
+   *    product of it with the known horizontal component of the earth's magnetic field. The result is a rotation error vector
+   *    that you transform back into the body frame, and use it to compensate for gyro drift. That is technique we are using in
+   *    MatrixPilot, it works for any orientation. A combination of two reference vectors (magnetometer and accelerometer) will
+   *     provide a 3 axis lock.
+   * 2. If you are using Euler angles to represent orientation, they do not work for 90 degree pitch. There is an effect known 
+   *    as "gimbal lock" that throws off the roll. It is better to use either an angle and rotation axis representation, or
+   *    quaternions.
+   *
+   * ummm... we're no actually calculating MAG_Heading anywhere... so it's zero...
+   * mag_earth[3x1] = mag[3x1] dot dcm[3x3]
+   * earth_rotation_error_vector = mag_earth[x and y] cross known_earth_mag[??]
+   * gyro drift error aka body_rotation_error_vector = earth_rotation_error_Vector times dcm[3x3]
+  float mag_earth[3], mag_sensor[3];
+  mag_sensor[0] = magnetom_x;
+  mag_sensor[1] = magnetom_y;
+  mag_sensor[2] = magnetom_z;
+  mag_earth[0] = VectorDotProduct( &dcm[0] , mag_sensor ) << 1;
+  mag_earth[1] = VectorDotProduct( &dcm[1] , mag_sensor ) << 1;
+  mag_earth[2] = VectorDotProduct( &dcm[2] , mag_sensor ) << 1;
+  mag_error = 100 * VectorDotProduct( 2 , mag_earth , declinationVector ) ; // Dotgain = 1/2
+  VectorScale( 3 , errorYawplane , &rmat[6] , mag_error ) ; // Scalegain = 1/2
+   */
+ 
+  mag_heading_x = cos(MAG_Heading);
+  mag_heading_y = sin(MAG_Heading);
+  errorCourse=(dcm[0][0]*mag_heading_y) - (dcm[1][0]*mag_heading_x);  //Calculating YAW error
+  Vector_Scale(errorYaw,&dcm[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+  
+  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+  Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
+  
+  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+  Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+
+/**************************************************/
+void DCM::Accel_adjust(void)
+{
+ Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]);  // Centrifugal force on Acc_y = GPS_speed*GyroZ
+ Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]);  // Centrifugal force on Acc_z = GPS_speed*GyroY
+ // Add linear (x-axis) acceleration correction here
+ 
+// from MatrixPilot
+// total (3D) airspeed in cm/sec is used to adjust for acceleration
+//gplane[0]=gplane[0]- omegaSOG( omegaAccum[2] , air_speed_3DGPS ) ;
+//gplane[2]=gplane[2]+ omegaSOG( omegaAccum[0] , air_speed_3DGPS ) ;
+//gplane[1]=gplane[1]+ ((unsigned int)(ACCELSCALE))*forward_acceleration 
+}
+
+/**************************************************/
+void DCM::Matrix_update(void)
+{
+  // TODO: Hardware-specific routines to convert gyro to units; this (probably) should be handled elsewhere
+
+  Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
+  Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
+  Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
+  
+  // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
+  Accel_Vector[0]=accel_x;
+  Accel_Vector[1]=accel_y;
+  Accel_Vector[2]=accel_z;
+    
+  Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+
+  // Remove centrifugal & linear acceleration.   
+  Accel_adjust();    
+  
+ #if OUTPUTMODE==1         
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+  Update_Matrix[2][2]=0;
+ #else                    // Uncorrected data (no drift correction)
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+  Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+  Update_Matrix[2][2]=0;
+ #endif
+
+  Matrix_Multiply(Temporary_Matrix, dcm, Update_Matrix); //c=a*b
+
+// ???  Matrix_Add(dcm, dcm, Temporary_Matrix); ???
+  for(int x=0; x<3; x++) { //Matrix Addition (update)
+    for(int y=0; y<3; y++) {
+      dcm[x][y] += Temporary_Matrix[x][y];
+    } 
+  }
+}
+
+void DCM::Euler_angles(void)
+{
+  pitch = -asin(dcm[2][0]);
+  roll = atan2(dcm[2][1],dcm[2][2]);
+  yaw = atan2(dcm[1][0],dcm[0][0]);
+}
+
+void DCM::Update_step()
+{         
+    Read_Gyro();
+    Read_Accel();
+    if (--update_count == 0) {
+        Update_mag();
+        update_count = MAG_SKIP;
+    }
+    Matrix_update();
+    Normalize();
+    Drift_correction();
+    //Accel_adjust();
+    Euler_angles();
+}
+
+void DCM::Update_mag()
+{
+    Read_Compass();
+    Compass_Heading();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DCM.h	Tue Jan 24 17:40:40 2012 +0000
@@ -0,0 +1,168 @@
+/*
+MinIMU-9-mbed-AHRS
+Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)
+
+Modified and ported to mbed environment by Michael Shimniok
+http://www.bot-thoughts.com/
+
+Basedd on MinIMU-9-Arduino-AHRS
+Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
+
+Copyright (c) 2011 Pololu Corporation.
+http://www.pololu.com/
+
+MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
+http://code.google.com/p/sf9domahrs/
+
+sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
+Julio and Doug Weibel:
+http://code.google.com/p/ardu-imu/
+
+MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
+under the terms of the GNU Lesser General Public License as published by the
+Free Software Foundation, either version 3 of the License, or (at your option)
+any later version.
+
+MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
+WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
+more details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
+
+*/
+#ifndef __DCM_H
+#define __DCM_H
+
+#define GRAVITY 1024  //this equivalent to 1G in the raw data coming from the accelerometer 
+#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
+
+#define ToRad(x) ((x)*0.01745329252)  // *pi/180
+#define ToDeg(x) ((x)*57.2957795131)  // *180/pi
+
+//#define Kp_ROLLPITCH 0.02
+//#define Ki_ROLLPITCH 0.00002
+//#define Kp_YAW 1.2
+//#define Ki_YAW 0.00002
+#define Kp_ROLLPITCH 0.02
+#define Ki_ROLLPITCH 0.00002
+#define Kp_YAW 1.2
+#define Ki_YAW 0.00002
+
+#define OUTPUTMODE 1 // enable drift correction
+
+// TODO: move elsewhere
+// Gyro sensor hardware-specific stuff
+#define Gyro_Gain_X 0.07 //X axis Gyro gain
+#define Gyro_Gain_Y 0.07 //Y axis Gyro gain
+#define Gyro_Gain_Z 0.07 //Z axis Gyro gain
+#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians per second
+#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians per second
+#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians per second
+
+/** DCM AHRS algorithm ported to mbed from Pololu MinIMU-9 in turn ported from ArduPilot 1.5
+ * revised in a more OO style, though no done yet.
+ *
+ * Early version. There's more to do here but it's a start, at least.
+ *
+ * Warning: Interface will most likely change.
+ *
+ * Expects to see a Sensors.h in your project, as follows, with functions that read sensors and set the appropriate 
+ * "input" member variables below. Eventually I'll change this to a better encapsulated OOP approach. 
+ *
+ * This approach of an external Sensors.* is an abstraction layer that makes it much, much easier to
+ * swap in a different sensor suite versus the original code. You can use Serial, I2C, SPI, Analog,
+ * whatever you want, whatever it takes as long as you populate the gyro, accel, and mag vectors before
+ * calling Update_step()
+ *
+ * @code
+ * #ifndef __SENSORS_H
+ * #define __SENSORS_H
+ *
+ * void Gyro_Init(void);
+ * void Read_Gyro(void);
+ * void Accel_Init(void);
+ * void Read_Accel(void);
+ * void Compass_Init(void);
+ * void Read_Compass(void);
+ * void Calculate_Offsets(void);
+ * void Compass_Heading(void);
+ * 
+ * #endif
+ * @endcode
+ *
+ */
+class DCM {
+    public:
+        /** Output: Euler angle: roll */
+        float roll;
+        /** Output:  Euler angle: pitch */
+        float pitch;
+        /** Output:  Euler angle: yaw */
+        float yaw;
+        
+        /** Input gyro sensor reading X-axis */
+        int gyro_x;
+        /** Input gyro sensor reading Y-axis */
+        int gyro_y;
+        /** Input gyro sensor reading Z-axis */
+        int gyro_z;
+        /** Input accelerometer sensor reading X-axis */
+        int accel_x;
+        /** Input accelerometer sensor reading Y-axis */
+        int accel_y;
+        /** Input accelerometer sensor reading Z-axis */
+        int accel_z;
+        /*
+        int magnetom_x;
+        int magnetom_y;
+        int magnetom_z;
+        */
+        /** Input for the offset corrected & scaled magnetometer reading, X-axis */
+        float c_magnetom_x;
+        /** Input for the offset corrected & scaled magnetometer reading, Y-axis */
+        float c_magnetom_y;
+        /** Input for the offset corrected & scaled magnetometer reading, Z-axis */
+        float c_magnetom_z;
+        /** Input for the calculated magnetic heading */
+        float MAG_Heading;
+        /** Input for the speed (ie, the magnitude of the 3d velocity vector */
+        float speed_3d;
+        /** Input for the integration time (DCM algorithm) */
+        float G_Dt;
+
+        /** Creates a new DCM AHRS algorithm object
+         */
+        DCM(void);
+
+        /** A single update cycle for the algorithm; updates the matrix, normalizes it, corrects for gyro
+         * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often
+         * than gyro/accelerometer-based update
+         */
+        void Update_step(void);
+        
+    private:        
+        float Accel_Vector[3];  // Store the acceleration in a vector
+        float Gyro_Vector[3];   // Store the gyros turn rate in a vector
+        float dcm[3][3];        // The Direction Cosine Matrix
+        float errorRollPitch[3]; 
+        float errorYaw[3];
+        float Omega_Vector[3];  // Corrected Gyro_Vector data
+        float Omega_P[3];       // Omega Proportional correction
+        float Omega_I[3];       // Omega Integrator
+        float Omega[3];         // Omega
+        float Temporary_Matrix[3][3];
+        float Update_Matrix[3][3];
+        int update_count;   // call Update_mag() every update_count calls to Update_step()
+        float constrain(float x, float a, float b);
+        void Normalize(void);
+        void Drift_correction(void);
+        void Accel_adjust(void);
+        void Matrix_update(void);
+        void Euler_angles(void);
+        void Update_mag(void);
+        
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.cpp	Tue Jan 24 17:40:40 2012 +0000
@@ -0,0 +1,54 @@
+void Vector_Cross_Product(float C[3], float A[3], float B[3])
+{
+    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
+    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
+    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
+  
+    return;
+}
+
+void Vector_Scale(float C[3], float A[3], float b)
+{
+    for (int m = 0; m < 3; m++)
+        C[m] = A[m] * b;
+        
+    return;
+}
+
+float Vector_Dot_Product(float A[3], float B[3])
+{
+    float result = 0.0;
+
+    for (int i = 0; i < 3; i++) {
+        result += A[i] * B[i];
+    }
+    
+    return result;
+}
+
+void Vector_Add(float C[3], float A[3], float B[3])
+{
+    for (int m = 0; m < 3; m++)
+        C[m] = A[m] + B[m];
+        
+    return;
+}
+
+void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
+{
+    for (int m = 0; m < 3; m++)
+        for (int n = 0; n < 3; n++)
+            C[m][n] = A[m][n] + B[m][n];
+}
+
+void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
+{
+    for (int i = 0; i < 3; i++) {
+        for (int j = 0; j < 3; j++) {
+            C[i][j] = 0;
+            for (int k = 0; k < 3; k++) {
+               C[i][j] += A[i][k] * B[k][j];
+            }
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.h	Tue Jan 24 17:40:40 2012 +0000
@@ -0,0 +1,39 @@
+#ifndef __MATRIX_H
+#define __MATRIX_H
+
+/** Take cross product of two 3x1 vectors: v1 x v2 = vectorOut
+ * @param v1 is the first vector
+ * @param v2 is the second vector
+ * @returns vectorOut = v1 x v2
+ */
+void Vector_Cross_Product(float vectorOut[3], float v1[3], float v2[3]);
+
+/** Multiple 3x1 vector by scalar: vectorOut = vectorIn times scale2
+ * @param vectorIn the vector
+ * @param scale2 is the scalar
+ * @returns vectorOut the result
+ */
+void Vector_Scale(float vectorOut[3], float vectorIn[3], float scale2);
+
+/** TDot product of two 3x1 vectors vector1 . vector2
+ * @param vector1 is the first vector
+ * @param vector2 is the second vector
+ * @returns float result of dot product
+ */
+float Vector_Dot_Product(float vector1[3], float vector2[3]);
+
+/** Adds two 3x1 vectors: vectorOut = vectorIn1 + vectorIn2
+ * @param vectorIn1 is the first vector
+ * @param vectorIn2 is the second vector
+ * @returns vectorOut is the result of the addition
+ */
+void Vector_Add(float vectorOut[3], float vectorIn1[3], float vectorIn2[3]);
+
+/** Multiplies two 3x3 matrices to get a third 3x3 matrix: c = ab
+ * @param a is the first vector
+ * @param b is the second vector
+ * @returns c as the result of the mutliplication
+ */
+void Matrix_Multiply(float c[3][3], float a[3][3], float b[3][3]);
+
+#endif
\ No newline at end of file