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?
User | Revision | Line number | New 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 |