Prosper Van / DCM_AHRS_GY80

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

Dependents:   quadCommand2

Fork of DCM_AHRS by Michael Shimniok

DCM.h@11:8d46121bd255, 2013-09-26 (annotated)

Committer:
oprospero
Date:
Thu Sep 26 05:19:13 2013 +0000
Revision:
11:8d46121bd255
Parent:
8:195d53710158
Child:
13:6134f21cad37
tweeks and test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:62284d27d75e 1 #ifndef __DCM_H
shimniok 0:62284d27d75e 2 #define __DCM_H
shimniok 0:62284d27d75e 3
shimniok 0:62284d27d75e 4
oprospero 5:d5ecdb82c17b 5 #define DEG2RAD(x) (x * 0.01745329252) // *pi/180
oprospero 5:d5ecdb82c17b 6 #define RAD2DEG(x) (x * 57.2957795131) // *180/pi
shimniok 0:62284d27d75e 7
oprospero 11:8d46121bd255 8 // Default values 0.2 0.00002 1.2 0.00002
oprospero 11:8d46121bd255 9 #define Kp_ROLLPITCH 0.1
oprospero 8:195d53710158 10 #define Ki_ROLLPITCH 0.00001
oprospero 8:195d53710158 11 #define Kp_YAW 0.1
shimniok 0:62284d27d75e 12 #define Ki_YAW 0.00002
shimniok 0:62284d27d75e 13
oprospero 11:8d46121bd255 14
shimniok 0:62284d27d75e 15 #define OUTPUTMODE 1 // enable drift correction
shimniok 0:62284d27d75e 16
shimniok 0:62284d27d75e 17 class DCM {
shimniok 0:62284d27d75e 18 public:
shimniok 0:62284d27d75e 19 /** Output: Euler angle: roll */
shimniok 0:62284d27d75e 20 float roll;
shimniok 0:62284d27d75e 21 /** Output: Euler angle: pitch */
shimniok 0:62284d27d75e 22 float pitch;
shimniok 0:62284d27d75e 23 /** Output: Euler angle: yaw */
shimniok 0:62284d27d75e 24 float yaw;
oprospero 11:8d46121bd255 25
shimniok 0:62284d27d75e 26 /** Input for the integration time (DCM algorithm) */
shimniok 0:62284d27d75e 27 float G_Dt;
shimniok 0:62284d27d75e 28
shimniok 0:62284d27d75e 29 /** Creates a new DCM AHRS algorithm object
shimniok 0:62284d27d75e 30 */
shimniok 0:62284d27d75e 31 DCM(void);
shimniok 0:62284d27d75e 32
shimniok 0:62284d27d75e 33 /** A single update cycle for the algorithm; updates the matrix, normalizes it, corrects for gyro
shimniok 0:62284d27d75e 34 * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often
shimniok 0:62284d27d75e 35 * than gyro/accelerometer-based update
shimniok 0:62284d27d75e 36 */
oprospero 4:4c6ffd1a88ce 37 void Update_step(float dt);
shimniok 0:62284d27d75e 38
oprospero 5:d5ecdb82c17b 39 float Accel_Vector[3]; // Store the acceleration in a vector
oprospero 5:d5ecdb82c17b 40 float Gyro_Vector[3]; // Store the gyros turn rate in a vector
oprospero 5:d5ecdb82c17b 41 float Magn_Vector[3];
oprospero 5:d5ecdb82c17b 42 float dcm[3][3]; // The Direction Cosine Matrix
oprospero 1:115cf0a84a9d 43 private:
oprospero 5:d5ecdb82c17b 44 /** Input for the calculated magnetic heading */
oprospero 5:d5ecdb82c17b 45 float MAG_Heading;
shimniok 0:62284d27d75e 46 float errorRollPitch[3];
shimniok 0:62284d27d75e 47 float errorYaw[3];
shimniok 0:62284d27d75e 48 float Omega_Vector[3]; // Corrected Gyro_Vector data
shimniok 0:62284d27d75e 49 float Omega_P[3]; // Omega Proportional correction
shimniok 0:62284d27d75e 50 float Omega_I[3]; // Omega Integrator
shimniok 0:62284d27d75e 51 float Omega[3]; // Omega
shimniok 0:62284d27d75e 52 float Temporary_Matrix[3][3];
shimniok 0:62284d27d75e 53 float Update_Matrix[3][3];
shimniok 0:62284d27d75e 54 int update_count; // call Update_mag() every update_count calls to Update_step()
shimniok 0:62284d27d75e 55 float constrain(float x, float a, float b);
shimniok 0:62284d27d75e 56 void Normalize(void);
shimniok 0:62284d27d75e 57 void Drift_correction(void);
shimniok 0:62284d27d75e 58 void Accel_adjust(void);
shimniok 0:62284d27d75e 59 void Matrix_update(void);
shimniok 0:62284d27d75e 60 void Euler_angles(void);
shimniok 0:62284d27d75e 61 void Update_mag(void);
oprospero 1:115cf0a84a9d 62 void Compass_Heading(void);
oprospero 6:49cbf2acc4e6 63 void reset_sensor_fusion(void);
oprospero 6:49cbf2acc4e6 64 void pv(float a[3]);
oprospero 6:49cbf2acc4e6 65 void pm(float a[3][3]);
shimniok 0:62284d27d75e 66
shimniok 0:62284d27d75e 67 };
shimniok 0:62284d27d75e 68
shimniok 0:62284d27d75e 69 #endif