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@14:6cb3c2204b9b, 2013-10-16 (annotated)

Committer:
oprospero
Date:
Wed Oct 16 07:49:34 2013 +0000
Revision:
14:6cb3c2204b9b
Parent:
13:6134f21cad37 
Fixed vector copy;

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 13:6134f21cad37 37 void Update_step(float dt, float AccelV[], float GyroV[], float MagnV[]);
oprospero 14:6cb3c2204b9b 38 void reset_sensor_fusion(float AccelV[], float GyroV[], float MagnV[]);
oprospero 13:6134f21cad37 39
oprospero 5:d5ecdb82c17b 40 float dcm[3][3]; // The Direction Cosine Matrix
oprospero 1:115cf0a84a9d 41 private:
oprospero 5:d5ecdb82c17b 42 /** Input for the calculated magnetic heading */
oprospero 13:6134f21cad37 43 float Accel_Vector[3];
oprospero 13:6134f21cad37 44 float Gyro_Vector[3];
oprospero 13:6134f21cad37 45 float Magn_Vector[3];
oprospero 5:d5ecdb82c17b 46 float MAG_Heading;
shimniok 0:62284d27d75e 47 float errorRollPitch[3];
shimniok 0:62284d27d75e 48 float errorYaw[3];
shimniok 0:62284d27d75e 49 float Omega_Vector[3]; // Corrected Gyro_Vector data
shimniok 0:62284d27d75e 50 float Omega_P[3]; // Omega Proportional correction
shimniok 0:62284d27d75e 51 float Omega_I[3]; // Omega Integrator
shimniok 0:62284d27d75e 52 float Omega[3]; // Omega
shimniok 0:62284d27d75e 53 float Temporary_Matrix[3][3];
shimniok 0:62284d27d75e 54 float Update_Matrix[3][3];
shimniok 0:62284d27d75e 55 int update_count; // call Update_mag() every update_count calls to Update_step()
shimniok 0:62284d27d75e 56 float constrain(float x, float a, float b);
oprospero 13:6134f21cad37 57
oprospero 13:6134f21cad37 58 void Matrix_update(void);
shimniok 0:62284d27d75e 59 void Normalize(void);
shimniok 0:62284d27d75e 60 void Drift_correction(void);
shimniok 0:62284d27d75e 61 void Accel_adjust(void);
shimniok 0:62284d27d75e 62 void Euler_angles(void);
shimniok 0:62284d27d75e 63 void Update_mag(void);
oprospero 1:115cf0a84a9d 64 void Compass_Heading(void);
oprospero 6:49cbf2acc4e6 65 void pv(float a[3]);
oprospero 6:49cbf2acc4e6 66 void pm(float a[3][3]);
shimniok 0:62284d27d75e 67
shimniok 0:62284d27d75e 68 };
shimniok 0:62284d27d75e 69
shimniok 0:62284d27d75e 70 #endif