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
Fork of DCM_AHRS by
Diff: DCM.h
- Revision:
- 13:6134f21cad37
- Parent:
- 11:8d46121bd255
- Child:
- 14:6cb3c2204b9b
--- a/DCM.h Mon Oct 14 02:44:11 2013 +0000 +++ b/DCM.h Tue Oct 15 08:24:34 2013 +0000 @@ -34,14 +34,14 @@ * drift on 3 axes, (does not yet adjust for acceleration). Magnetometer update is run less often * than gyro/accelerometer-based update */ - void Update_step(float dt); - - float Accel_Vector[3]; // Store the acceleration in a vector - float Gyro_Vector[3]; // Store the gyros turn rate in a vector - float Magn_Vector[3]; + void Update_step(float dt, float AccelV[], float GyroV[], float MagnV[]); + float dcm[3][3]; // The Direction Cosine Matrix private: /** Input for the calculated magnetic heading */ + float Accel_Vector[3]; + float Gyro_Vector[3]; + float Magn_Vector[3]; float MAG_Heading; float errorRollPitch[3]; float errorYaw[3]; @@ -53,10 +53,11 @@ 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 Matrix_update(void); void Normalize(void); void Drift_correction(void); void Accel_adjust(void); - void Matrix_update(void); void Euler_angles(void); void Update_mag(void); void Compass_Heading(void);