Diff: DCM.h

Revision:

0:62284d27d75e

--- /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