DCM.h

00001 /*
00002 MinIMU-9-mbed-AHRS
00003 Pololu MinIMU-9 + mbed AHRS (Attitude and Heading Reference System)
00004 
00005 Modified and ported to mbed environment by Michael Shimniok
00006 http://www.bot-thoughts.com/
00007 
00008 Basedd on MinIMU-9-Arduino-AHRS
00009 Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
00010 
00011 Copyright (c) 2011 Pololu Corporation.
00012 http://www.pololu.com/
00013 
00014 MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
00015 http://code.google.com/p/sf9domahrs/
00016 
00017 sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
00018 Julio and Doug Weibel:
00019 http://code.google.com/p/ardu-imu/
00020 
00021 MinIMU-9-Arduino-AHRS is free software: you can redistribute it and/or modify it
00022 under the terms of the GNU Lesser General Public License as published by the
00023 Free Software Foundation, either version 3 of the License, or (at your option)
00024 any later version.
00025 
00026 MinIMU-9-Arduino-AHRS is distributed in the hope that it will be useful, but
00027 WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00028 FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for
00029 more details.
00030 
00031 You should have received a copy of the GNU Lesser General Public License along
00032 with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
00033 
00034 */
00035 #ifndef __DCM_H
00036 #define __DCM_H
00037 
00038 #define GRAVITY 1024  //this equivalent to 1G in the raw data coming from the accelerometer 
00039 #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
00040 
00041 #define ToRad(x) ((x)*0.01745329252)  // *pi/180
00042 #define ToDeg(x) ((x)*57.2957795131)  // *180/pi
00043 
00044 //#define Kp_ROLLPITCH 0.02
00045 //#define Ki_ROLLPITCH 0.00002
00046 //#define Kp_YAW 1.2
00047 //#define Ki_YAW 0.00002
00048 #define Kp_ROLLPITCH 0.02
00049 #define Ki_ROLLPITCH 0.00002
00050 #define Kp_YAW 1.2
00051 #define Ki_YAW 0.00002
00052 
00053 #define OUTPUTMODE 1 // enable drift correction
00054 
00055 // TODO: move elsewhere
00056 // Gyro sensor hardware-specific stuff
00057 #define Gyro_Gain_X 0.07 //X axis Gyro gain
00058 #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
00059 #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
00060 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians per second
00061 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians per second
00062 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians per second
00063 
00064 /** DCM AHRS algorithm ported to mbed from Pololu MinIMU-9 in turn ported from ArduPilot 1.5
00065  * revised in a more OO style, though no done yet.
00066  *
00067  * Early version. There's more to do here but it's a start, at least.
00068  *
00069  * Warning: Interface will most likely change.
00070  *
00071  * Expects to see a Sensors.h in your project, as follows, with functions that read sensors and set the appropriate 
00072  * "input" member variables below. Eventually I'll change this to a better encapsulated OOP approach. 
00073  *
00074  * This approach of an external Sensors.* is an abstraction layer that makes it much, much easier to
00075  * swap in a different sensor suite versus the original code. You can use Serial, I2C, SPI, Analog,
00076  * whatever you want, whatever it takes as long as you populate the gyro, accel, and mag vectors before
00077  * calling Update_step()
00078  *
00079  * @code
00080  * #ifndef __SENSORS_H
00081  * #define __SENSORS_H
00082  *
00083  * void Gyro_Init(void);
00084  * void Read_Gyro(void);
00085  * void Accel_Init(void);
00086  * void Read_Accel(void);
00087  * void Compass_Init(void);
00088  * void Read_Compass(void);
00089  * void Calculate_Offsets(void);
00090  * void Compass_Heading(void);
00091  * 
00092  * #endif
00093  * @endcode
00094  *
00095  */
00096 class DCM {
00097     public:
00098         /** Output: Euler angle: roll */
00099         float roll;
00100         /** Output:  Euler angle: pitch */
00101         float pitch;
00102         /** Output:  Euler angle: yaw */
00103         float yaw;
00104         
00105         /** Input gyro sensor reading X-axis */
00106         int gyro_x;
00107         /** Input gyro sensor reading Y-axis */
00108         int gyro_y;
00109         /** Input gyro sensor reading Z-axis */
00110         int gyro_z;
00111         /** Input accelerometer sensor reading X-axis */
00112         int accel_x;
00113         /** Input accelerometer sensor reading Y-axis */
00114         int accel_y;
00115         /** Input accelerometer sensor reading Z-axis */
00116         int accel_z;
00117         /*
00118         int magnetom_x;
00119         int magnetom_y;
00120         int magnetom_z;
00121         */
00122         /** Input for the offset corrected & scaled magnetometer reading, X-axis */
00123         float c_magnetom_x;
00124         /** Input for the offset corrected & scaled magnetometer reading, Y-axis */
00125         float c_magnetom_y;
00126         /** Input for the offset corrected & scaled magnetometer reading, Z-axis */
00127         float c_magnetom_z;
00128         /** Input for the calculated magnetic heading */
00129         float MAG_Heading;
00130         /** Input for the speed (ie, the magnitude of the 3d velocity vector */
00131         float speed_3d;
00132         /** Input for the integration time (DCM algorithm) */
00133         float G_Dt;
00134 
00135         /** Creates a new DCM AHRS algorithm object
00136          */
00137         DCM(void);
00138 
00139         /** A single update cycle for the algorithm; updates the matrix, normalizes it, corrects for gyro
00140          * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often
00141          * than gyro/accelerometer-based update
00142          */
00143         void Update_step(void);
00144         
00145     private:        
00146         float Accel_Vector[3];  // Store the acceleration in a vector
00147         float Gyro_Vector[3];   // Store the gyros turn rate in a vector
00148         float dcm[3][3];        // The Direction Cosine Matrix
00149         float errorRollPitch[3]; 
00150         float errorYaw[3];
00151         float Omega_Vector[3];  // Corrected Gyro_Vector data
00152         float Omega_P[3];       // Omega Proportional correction
00153         float Omega_I[3];       // Omega Integrator
00154         float Omega[3];         // Omega
00155         float Temporary_Matrix[3][3];
00156         float Update_Matrix[3][3];
00157         int update_count;   // call Update_mag() every update_count calls to Update_step()
00158         float constrain(float x, float a, float b);
00159         void Normalize(void);
00160         void Drift_correction(void);
00161         void Accel_adjust(void);
00162         void Matrix_update(void);
00163         void Euler_angles(void);
00164         void Update_mag(void);
00165         
00166 };
00167 
00168 #endif