Michael Shimniok / DCM_AHRS

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

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

Committer:
shimniok
Date:
Tue Jan 24 17:40:40 2012 +0000
Revision:
0:62284d27d75e
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 #ifndef __DCM_H
shimniok 0:62284d27d75e 36 #define __DCM_H
shimniok 0:62284d27d75e 37
shimniok 0:62284d27d75e 38 #define GRAVITY 1024 //this equivalent to 1G in the raw data coming from the accelerometer
shimniok 0:62284d27d75e 39 #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
shimniok 0:62284d27d75e 40
shimniok 0:62284d27d75e 41 #define ToRad(x) ((x)*0.01745329252) // *pi/180
shimniok 0:62284d27d75e 42 #define ToDeg(x) ((x)*57.2957795131) // *180/pi
shimniok 0:62284d27d75e 43
shimniok 0:62284d27d75e 44 //#define Kp_ROLLPITCH 0.02
shimniok 0:62284d27d75e 45 //#define Ki_ROLLPITCH 0.00002
shimniok 0:62284d27d75e 46 //#define Kp_YAW 1.2
shimniok 0:62284d27d75e 47 //#define Ki_YAW 0.00002
shimniok 0:62284d27d75e 48 #define Kp_ROLLPITCH 0.02
shimniok 0:62284d27d75e 49 #define Ki_ROLLPITCH 0.00002
shimniok 0:62284d27d75e 50 #define Kp_YAW 1.2
shimniok 0:62284d27d75e 51 #define Ki_YAW 0.00002
shimniok 0:62284d27d75e 52
shimniok 0:62284d27d75e 53 #define OUTPUTMODE 1 // enable drift correction
shimniok 0:62284d27d75e 54
shimniok 0:62284d27d75e 55 // TODO: move elsewhere
shimniok 0:62284d27d75e 56 // Gyro sensor hardware-specific stuff
shimniok 0:62284d27d75e 57 #define Gyro_Gain_X 0.07 //X axis Gyro gain
shimniok 0:62284d27d75e 58 #define Gyro_Gain_Y 0.07 //Y axis Gyro gain
shimniok 0:62284d27d75e 59 #define Gyro_Gain_Z 0.07 //Z axis Gyro gain
shimniok 0:62284d27d75e 60 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians per second
shimniok 0:62284d27d75e 61 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians per second
shimniok 0:62284d27d75e 62 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians per second
shimniok 0:62284d27d75e 63
shimniok 0:62284d27d75e 64 /** DCM AHRS algorithm ported to mbed from Pololu MinIMU-9 in turn ported from ArduPilot 1.5
shimniok 0:62284d27d75e 65 * revised in a more OO style, though no done yet.
shimniok 0:62284d27d75e 66 *
shimniok 0:62284d27d75e 67 * Early version. There's more to do here but it's a start, at least.
shimniok 0:62284d27d75e 68 *
shimniok 0:62284d27d75e 69 * Warning: Interface will most likely change.
shimniok 0:62284d27d75e 70 *
shimniok 0:62284d27d75e 71 * Expects to see a Sensors.h in your project, as follows, with functions that read sensors and set the appropriate
shimniok 0:62284d27d75e 72 * "input" member variables below. Eventually I'll change this to a better encapsulated OOP approach.
shimniok 0:62284d27d75e 73 *
shimniok 0:62284d27d75e 74 * This approach of an external Sensors.* is an abstraction layer that makes it much, much easier to
shimniok 0:62284d27d75e 75 * swap in a different sensor suite versus the original code. You can use Serial, I2C, SPI, Analog,
shimniok 0:62284d27d75e 76 * whatever you want, whatever it takes as long as you populate the gyro, accel, and mag vectors before
shimniok 0:62284d27d75e 77 * calling Update_step()
shimniok 0:62284d27d75e 78 *
shimniok 0:62284d27d75e 79 * @code
shimniok 0:62284d27d75e 80 * #ifndef __SENSORS_H
shimniok 0:62284d27d75e 81 * #define __SENSORS_H
shimniok 0:62284d27d75e 82 *
shimniok 0:62284d27d75e 83 * void Gyro_Init(void);
shimniok 0:62284d27d75e 84 * void Read_Gyro(void);
shimniok 0:62284d27d75e 85 * void Accel_Init(void);
shimniok 0:62284d27d75e 86 * void Read_Accel(void);
shimniok 0:62284d27d75e 87 * void Compass_Init(void);
shimniok 0:62284d27d75e 88 * void Read_Compass(void);
shimniok 0:62284d27d75e 89 * void Calculate_Offsets(void);
shimniok 0:62284d27d75e 90 * void Compass_Heading(void);
shimniok 0:62284d27d75e 91 *
shimniok 0:62284d27d75e 92 * #endif
shimniok 0:62284d27d75e 93 * @endcode
shimniok 0:62284d27d75e 94 *
shimniok 0:62284d27d75e 95 */
shimniok 0:62284d27d75e 96 class DCM {
shimniok 0:62284d27d75e 97 public:
shimniok 0:62284d27d75e 98 /** Output: Euler angle: roll */
shimniok 0:62284d27d75e 99 float roll;
shimniok 0:62284d27d75e 100 /** Output: Euler angle: pitch */
shimniok 0:62284d27d75e 101 float pitch;
shimniok 0:62284d27d75e 102 /** Output: Euler angle: yaw */
shimniok 0:62284d27d75e 103 float yaw;
shimniok 0:62284d27d75e 104
shimniok 0:62284d27d75e 105 /** Input gyro sensor reading X-axis */
shimniok 0:62284d27d75e 106 int gyro_x;
shimniok 0:62284d27d75e 107 /** Input gyro sensor reading Y-axis */
shimniok 0:62284d27d75e 108 int gyro_y;
shimniok 0:62284d27d75e 109 /** Input gyro sensor reading Z-axis */
shimniok 0:62284d27d75e 110 int gyro_z;
shimniok 0:62284d27d75e 111 /** Input accelerometer sensor reading X-axis */
shimniok 0:62284d27d75e 112 int accel_x;
shimniok 0:62284d27d75e 113 /** Input accelerometer sensor reading Y-axis */
shimniok 0:62284d27d75e 114 int accel_y;
shimniok 0:62284d27d75e 115 /** Input accelerometer sensor reading Z-axis */
shimniok 0:62284d27d75e 116 int accel_z;
shimniok 0:62284d27d75e 117 /*
shimniok 0:62284d27d75e 118 int magnetom_x;
shimniok 0:62284d27d75e 119 int magnetom_y;
shimniok 0:62284d27d75e 120 int magnetom_z;
shimniok 0:62284d27d75e 121 */
shimniok 0:62284d27d75e 122 /** Input for the offset corrected & scaled magnetometer reading, X-axis */
shimniok 0:62284d27d75e 123 float c_magnetom_x;
shimniok 0:62284d27d75e 124 /** Input for the offset corrected & scaled magnetometer reading, Y-axis */
shimniok 0:62284d27d75e 125 float c_magnetom_y;
shimniok 0:62284d27d75e 126 /** Input for the offset corrected & scaled magnetometer reading, Z-axis */
shimniok 0:62284d27d75e 127 float c_magnetom_z;
shimniok 0:62284d27d75e 128 /** Input for the calculated magnetic heading */
shimniok 0:62284d27d75e 129 float MAG_Heading;
shimniok 0:62284d27d75e 130 /** Input for the speed (ie, the magnitude of the 3d velocity vector */
shimniok 0:62284d27d75e 131 float speed_3d;
shimniok 0:62284d27d75e 132 /** Input for the integration time (DCM algorithm) */
shimniok 0:62284d27d75e 133 float G_Dt;
shimniok 0:62284d27d75e 134
shimniok 0:62284d27d75e 135 /** Creates a new DCM AHRS algorithm object
shimniok 0:62284d27d75e 136 */
shimniok 0:62284d27d75e 137 DCM(void);
shimniok 0:62284d27d75e 138
shimniok 0:62284d27d75e 139 /** A single update cycle for the algorithm; updates the matrix, normalizes it, corrects for gyro
shimniok 0:62284d27d75e 140 * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often
shimniok 0:62284d27d75e 141 * than gyro/accelerometer-based update
shimniok 0:62284d27d75e 142 */
shimniok 0:62284d27d75e 143 void Update_step(void);
shimniok 0:62284d27d75e 144
shimniok 0:62284d27d75e 145 private:
shimniok 0:62284d27d75e 146 float Accel_Vector[3]; // Store the acceleration in a vector
shimniok 0:62284d27d75e 147 float Gyro_Vector[3]; // Store the gyros turn rate in a vector
shimniok 0:62284d27d75e 148 float dcm[3][3]; // The Direction Cosine Matrix
shimniok 0:62284d27d75e 149 float errorRollPitch[3];
shimniok 0:62284d27d75e 150 float errorYaw[3];
shimniok 0:62284d27d75e 151 float Omega_Vector[3]; // Corrected Gyro_Vector data
shimniok 0:62284d27d75e 152 float Omega_P[3]; // Omega Proportional correction
shimniok 0:62284d27d75e 153 float Omega_I[3]; // Omega Integrator
shimniok 0:62284d27d75e 154 float Omega[3]; // Omega
shimniok 0:62284d27d75e 155 float Temporary_Matrix[3][3];
shimniok 0:62284d27d75e 156 float Update_Matrix[3][3];
shimniok 0:62284d27d75e 157 int update_count; // call Update_mag() every update_count calls to Update_step()
shimniok 0:62284d27d75e 158 float constrain(float x, float a, float b);
shimniok 0:62284d27d75e 159 void Normalize(void);
shimniok 0:62284d27d75e 160 void Drift_correction(void);
shimniok 0:62284d27d75e 161 void Accel_adjust(void);
shimniok 0:62284d27d75e 162 void Matrix_update(void);
shimniok 0:62284d27d75e 163 void Euler_angles(void);
shimniok 0:62284d27d75e 164 void Update_mag(void);
shimniok 0:62284d27d75e 165
shimniok 0:62284d27d75e 166 };
shimniok 0:62284d27d75e 167
shimniok 0:62284d27d75e 168 #endif