Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: attitude.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/attitude.c Fri Feb 25 01:35:24 2011 +0000
+++ b/attitude.c Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
// = UAVXArm Quadrocopter Controller =
// = Copyright (c) 2008 by Prof. Greg Egan =
// = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
-// = http://code.google.com/p/uavp-mods/ http://uavp.ch =
+// = http://code.google.com/p/uavp-mods/ =
// ===============================================================================================
// This is part of UAVXArm.
@@ -20,53 +20,85 @@
#include "UAVXArm.h"
-//#define USE_GYRO_RATE
-
// Reference frame is positive X forward, Y left, Z down, Roll right, Pitch up, Yaw CW.
+// CAUTION: Because of the coordinate frame the LR Acc sense must be negated for roll compensation.
-void AttitudeFailsafeEstimate(void);
-void DoLegacyYawComp(void);
+void AdaptiveYawLPFreq(void);
+void DoLegacyYawComp(uint8);
+void NormaliseAccelerations(void);
void AttitudeTest(void);
+void InitAttitude(void);
-real32 dT, HalfdT, dTR, dTmS;
+real32 AccMagnitude;
+real32 EstAngle[3][MaxAttitudeScheme];
+real32 EstRate[3][MaxAttitudeScheme];
+real32 Correction[2];
+real32 YawFilterLPFreq;
+real32 dT, dTOn2, dTR, dTmS;
uint32 uSp;
-uint8 AttitudeMethod = PremerlaniDCM; //MadgwickIMU PremerlaniDCM MadgwickAHRS;
-void AttitudeFailsafeEstimate(void) {
+uint8 AttitudeMethod = Wolferl; //Integrator, Wolferl MadgwickIMU PremerlaniDCM MadgwickAHRS, MultiWii;
+
+void AdaptiveYawLPFreq(void) { // Filter LP freq is decreased with reduced yaw stick deflection
- static uint8 i;
+ YawFilterLPFreq = ( MAX_YAW_FREQ*abs(DesiredYaw) / RC_NEUTRAL );
+ YawFilterLPFreq = Limit(YawFilterLPFreq, 0.5, MAX_YAW_FREQ);
+
+} // AdaptiveYawFilterA
- for ( i = 0; i < (uint8)3; i++ ) {
- Rate[i] = Gyro[i];
- Angle[i] += Rate[i] * dTmS;
- }
-} // AttitudeFailsafeEstimate
+real32 HE;
+
+void DoLegacyYawComp(uint8 S) {
-void DoLegacyYawComp(void) {
+#define COMPASS_MIDDLE 10 // yaw stick neutral dead zone
+#define DRIFT_COMP_YAW_RATE QUARTERPI // Radians/Sec
- static real32 Temp, HE;
+ static int16 Temp;
+
+ // Yaw Angle here is meant to be interpreted as the Heading Error
+
+ Rate[Yaw] = Gyro[Yaw];
- if ( F.CompassValid ) {
- // + CCW
- Temp = DesiredYaw - Trim[Yaw];
- if ( fabs(Temp) > COMPASS_MIDDLE ) { // acquire new heading
- DesiredHeading = Heading;
- HE = 0.0;
+ Temp = DesiredYaw - Trim[Yaw];
+ if ( F.CompassValid ) // CW+
+ if ( abs(Temp) > COMPASS_MIDDLE ) {
+ DesiredHeading = Heading; // acquire new heading
+ Angle[Yaw] = 0.0;
} else {
- HE = MakePi(DesiredHeading - Heading);
- HE = Limit(HE, -SIXTHPI, SIXTHPI); // 30 deg limit
- HE = HE * K[CompassKp];
- Rate[Yaw] -= Limit(HE, -COMPASS_MAXDEV, COMPASS_MAXDEV);
+ HE = MinimumTurn(DesiredHeading - Heading);
+ HE = Limit1(HE, SIXTHPI); // 30 deg limit
+ HE = HE*K[CompassKp];
+ Rate[Yaw] = -Limit1(HE, DRIFT_COMP_YAW_RATE);
}
+ else {
+ DesiredHeading = Heading;
+ Angle[Yaw] = 0.0;
}
- Angle[Yaw] += Rate[Yaw];
- Angle[Yaw] = Limit(Angle[Yaw], -K[YawIntLimit], K[YawIntLimit]);
-
- Angle[Yaw] = DecayX(Angle[Yaw], 0.0002); // GKE added to kill gyro drift
+ Angle[Yaw] += Rate[Yaw]*dT;
+ // Angle[Yaw] = Limit1(Angle[Yaw], K[YawIntLimit]);
} // DoLegacyYawComp
+void NormaliseAccelerations(void) {
+
+ const real32 MIN_ACC_MAGNITUDE = 0.7; // below this the accelerometers are deemed unreliable - falling?
+
+ static real32 ReNorm;
+
+ AccMagnitude = sqrt(Sqr(Acc[BF]) + Sqr(Acc[LR]) + Sqr(Acc[UD]));
+ F.AccMagnitudeOK = AccMagnitude > MIN_ACC_MAGNITUDE;
+ if ( F.AccMagnitudeOK ) {
+ ReNorm = 1.0 / AccMagnitude;
+ Acc[BF] *= ReNorm;
+ Acc[LR] *= ReNorm;
+ Acc[UD] *= ReNorm;
+ } else {
+ Acc[LR] = Acc[BF] = 0.0;
+ Acc[UD] = 1.0;
+ }
+} // NormaliseAccelerations
+
void GetAttitude(void) {
static uint32 Now;
@@ -79,24 +111,18 @@
GetAccelerations();
}
- if ( mSClock() > mS[CompassUpdate] ) {
- mS[CompassUpdate] = mSClock() + COMPASS_UPDATE_MS;
- GetHeading();
-#ifndef USE_DCM_YAW
- DoLegacyYawComp();
-#endif // !USE_DCM_YAW
- }
-
Now = uSClock();
- dT = ( Now - uSp) * 0.000001;
- HalfdT = 0.5 * dT;
+ dT = ( Now - uSp)*0.000001;
+ dTOn2 = 0.5 * dT;
dTR = 1.0 / dT;
uSp = Now;
+ GetHeading(); // only updated every 50mS but read continuously anyway
+
if ( GyroType == IRSensors ) {
for ( i = 0; i < (uint8)2; i++ ) {
- Rate[i] = ( Angle[i] - Anglep[i] ) * dT;
+ Rate[i] = ( Angle[i] - Anglep[i] )*dT;
Anglep[i] = Angle[i];
}
@@ -104,23 +130,46 @@
} else {
DebugPin = true;
- switch ( AttitudeMethod ) {
- case PremerlaniDCM:
- DCMUpdate();
- DCMNormalise();
- DCMDriftCorrection();
- DCMEulerAngles();
- break;
- case MadgwickIMU:
- IMUupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD]);
- EulerAngles();
- // DoLegacyYawComp();
- break;
- case MadgwickAHRS: // must have magnetometer
- AHRSupdate(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], Acc[LR], Acc[UD], 1,0,0);//Mag[BF].V, Mag[LR].V, Mag[UD].V);
- EulerAngles();
- break;
- } // switch
+
+ NormaliseAccelerations(); // rudimentary check for free fall etc
+
+ // Wolferl
+ DoWolferl();
+
+#ifdef INC_ALL_SCHEMES
+
+ // Complementary
+ DoCF();
+
+ // Kalman
+ DoKalman();
+
+ //Premerlani DCM
+ DoDCM();
+
+ // MultiWii
+ DoMultiWii();
+
+ // Madgwick IMU
+ // DoMadgwickIMU(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]);
+
+ //#define INC_IMU2
+
+#ifdef INC_IMU2
+ DoMadgwickIMU2(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD]);
+#else
+ Madgwick IMU April 30, 2010 Paper Version
+#endif
+ // Madgwick AHRS BROKEN
+ DoMadgwickAHRS(Gyro[Roll], Gyro[Pitch], Gyro[Yaw], Acc[BF], -Acc[LR], -Acc[UD], Mag[BF].V, Mag[LR].V, -Mag[UD].V);
+
+ // Integrator - REFERENCE/FALLBACK
+ DoIntegrator();
+
+#endif // INC_ALL_SCHEMES
+
+ Angle[Roll] = EstAngle[Roll][AttitudeMethod];
+ Angle[Pitch] = EstAngle[Pitch][AttitudeMethod];
DebugPin = false;
}
@@ -129,82 +178,105 @@
} // GetAttitude
-
-void AttitudeTest(void) {
-
- TxString("\r\nAttitude Test\r\n");
+//____________________________________________________________________________________________
- GetAttitude();
+// Integrator
- TxString("\r\ndT \t");
- TxVal32(dT * 1000.0, 3, 0);
- TxString(" Sec.\r\n\r\n");
-
- if ( GyroType == IRSensors ) {
+void DoIntegrator(void) {
- TxString("IR Sensors:\r\n");
- TxString("\tRoll \t");
- TxVal32(IR[Roll] * 100.0, 2, HT);
- TxNextLine();
- TxString("\tPitch\t");
- TxVal32(IR[Pitch] * 100.0, 2, HT);
- TxNextLine();
- TxString("\tZ \t");
- TxVal32(IR[Yaw] * 100.0, 2, HT);
- TxNextLine();
- TxString("\tMin/Max\t");
- TxVal32(IRMin * 100.0, 2, HT);
- TxVal32(IRMax * 100.0, 2, HT);
- TxString("\tSwing\t");
- TxVal32(IRSwing * 100.0, 2, HT);
- TxNextLine();
-
- } else {
+ static uint8 g;
- TxString("Rates (Raw and Compensated):\r\n");
- TxString("\tRoll \t");
- TxVal32(Gyro[Roll] * MILLIANGLE, 3, HT);
- TxVal32(Rate[Roll] * MILLIANGLE, 3, 0);
- TxNextLine();
- TxString("\tPitch\t");
- TxVal32(Gyro[Pitch] * MILLIANGLE, 3, HT);
- TxVal32(Rate[Pitch] * MILLIANGLE, 3, 0);
- TxNextLine();
- TxString("\tYaw \t");
- TxVal32(Gyro[Yaw] * MILLIANGLE, 3, HT);
- TxVal32(Rate[Yaw] * MILLIANGLE, 3, 0);
- TxNextLine();
-
- TxString("Accelerations:\r\n");
- TxString("\tB->F \t");
- TxVal32(Acc[BF] * 1000.0, 3, 0);
- TxNextLine();
- TxString("\tL->R \t");
- TxVal32(Acc[LR] * 1000.0, 3, 0);
- TxNextLine();
- TxString("\tU->D \t");
- TxVal32(Acc[UD] * 1000.0, 3, 0);
- TxNextLine();
+ for ( g = 0; g < (uint8)3; g++ ) {
+ EstRate[g][Integrator] = Gyro[g];
+ EstAngle[g][Integrator] += EstRate[g][Integrator]*dT;
+ // EstAngle[g][Integrator] = DecayX(EstAngle[g][Integrator], 0.0001*dT);
}
- if ( CompassType != HMC6352 ) {
- TxString("Magnetic:\r\n");
- TxString("\tX \t");
- TxVal32(Mag[Roll].V, 0, 0);
- TxNextLine();
- TxString("\tY \t");
- TxVal32(Mag[Pitch].V, 0, 0);
- TxNextLine();
- TxString("\tZ \t");
- TxVal32(Mag[Yaw].V, 0, 0);
- TxNextLine();
+} // DoIntegrator
+
+//____________________________________________________________________________________________
+
+// Original simple accelerometer compensation of gyros developed for UAVP by Wolfgang Mahringer
+// and adapted for UAVXArm
+
+void DoWolferl(void) { // NO YAW ESTIMATE
+
+ const real32 WKp = 0.13; // 0.1
+
+ static real32 Grav[2], Dyn[2];
+ static real32 CompStep;
+
+ Rate[Roll] = Gyro[Roll];
+ Rate[Pitch] = Gyro[Pitch];
+
+ if ( F.AccMagnitudeOK ) {
+
+ CompStep = WKp*dT;
+
+ // Roll
+
+ Grav[LR] = -sin(EstAngle[Roll][Wolferl]); // original used approximation for small angles
+ Dyn[LR] = 0.0; //Rate[Roll]; // lateral acceleration due to rate - do later:).
+
+ Correction[LR] = -Acc[LR] + Grav[LR] + Dyn[LR]; // Acc is reversed
+ Correction[LR] = Limit1(Correction[LR], CompStep);
+
+ EstAngle[Roll][Wolferl] += Rate[Roll]*dT;
+ EstAngle[Roll][Wolferl] += Correction[LR];
+
+ // Pitch
+
+ Grav[BF] = -sin(EstAngle[Pitch][Wolferl]);
+ Dyn[BF] = 0.0; // Rate[Pitch];
+
+ Correction[BF] = Acc[BF] + Grav[BF] + Dyn[BF];
+ Correction[BF] = Limit1(Correction[BF], CompStep);
+
+ EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT;
+ EstAngle[Pitch][Wolferl] += Correction[BF];
+
+ } else {
+
+ EstAngle[Roll][Wolferl] += Rate[Roll]*dT;
+ EstAngle[Pitch][Wolferl] += Rate[Pitch]*dT;
+
}
- TxString("Heading: \t");
- TxVal32(Make2Pi(Heading) * MILLIANGLE, 3, 0);
- TxNextLine();
+} // DoWolferl
+
+//_________________________________________________________________________________
+
+// Complementary Filter originally authored by RoyLB
+// http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
+
+const real32 TauCF = 1.1;
+
+real32 AngleCF[2] = {0,0};
+real32 F0[2] = {0,0};
+real32 F1[2] = {0,0};
+real32 F2[2] = {0,0};
+
+real32 CF(uint8 a, real32 NewAngle, real32 NewRate) {
-} // AttitudeTest
+ if ( F.AccMagnitudeOK ) {
+ F0[a] = (NewAngle - AngleCF[a])*Sqr(TauCF);
+ F2[a] += F0[a]*dT;
+ F1[a] = F2[a] + (NewAngle - AngleCF[a])*2.0*TauCF + NewRate;
+ AngleCF[a] = (F1[a]*dT) + AngleCF[a];
+ } else
+ AngleCF[a] += NewRate*dT;
+
+ return ( AngleCF[a] ); // This is actually the current angle, but is stored for the next iteration
+} // CF
+
+void DoCF(void) { // NO YAW ANGLE ESTIMATE
+
+ EstAngle[Roll][Complementary] = CF(Roll, asin(-Acc[LR]), Gyro[Roll]);
+ EstAngle[Pitch][Complementary] = CF(Pitch, asin(-Acc[BF]), Gyro[Pitch]); // zzz minus???
+ EstRate[Roll][Complementary] = Gyro[Roll];
+ EstRate[Pitch][Complementary] = Gyro[Pitch];
+
+} // DoCF
//____________________________________________________________________________________________
@@ -212,19 +284,14 @@
// Direction Cosine Matrix IMU: Theory, Draft 17 June 2009. This paper draws upon the original
// work by R. Mahony et al. - Thanks Rob!
+// SEEMS TO BE A FAIRLY LARGE PHASE DELAY OF 2 SAMPLE INTERVALS
+
void DCMNormalise(void);
void DCMDriftCorrection(void);
void DCMMotionCompensation(void);
void DCMUpdate(void);
void DCMEulerAngles(void);
-// requires rescaling for the much faster PID cycle in UAVXArm
-// guess x5 initially
-#define Kp_RollPitch 25.0 // 5.0
-#define Ki_RollPitch 0.025 // 0.005
-#define Kp_Yaw 1.2
-#define Ki_Yaw 0.00002
-
real32 RollPitchError[3] = {0,0,0};
real32 OmegaV[3] = {0,0,0}; // corrected gyro data
real32 OmegaP[3] = {0,0,0}; // proportional correction
@@ -233,6 +300,7 @@
real32 DCM[3][3] = {{1,0,0 },{0,1,0} ,{0,0,1}};
real32 U[3][3] = {{0,1,2},{ 3,4,5} ,{6,7,8}};
real32 TempM[3][3] = {{0,0,0},{0,0,0},{ 0,0,0}};
+real32 AccV[3];
void DCMNormalise(void) {
@@ -241,7 +309,7 @@
static boolean Problem;
static uint8 r;
- Error = -VDot(&DCM[0][0], &DCM[1][0]) * 0.5; //eq.19
+ Error = -VDot(&DCM[0][0], &DCM[1][0])*0.5; //eq.19
VScale(&TempM[0][0], &DCM[1][0], Error); //eq.19
VScale(&TempM[1][0], &DCM[0][0], Error); //eq.19
@@ -249,13 +317,13 @@
VAdd(&TempM[0][0], &TempM[0][0], &DCM[0][0]); //eq.19
VAdd(&TempM[1][0], &TempM[1][0], &DCM[1][0]); //eq.19
- VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]); // c= a * b eq.20
+ VCross(&TempM[2][0],&TempM[0][0], &TempM[1][0]); // c= a*b eq.20
Problem = false;
for ( r = 0; r < (uint8)3; r++ ) {
- Renorm = VDot(&TempM[r][0],&TempM[r][0]);
+ Renorm = VDot(&TempM[r][0], &TempM[r][0]);
if ( (Renorm < 1.5625) && (Renorm > 0.64) )
- Renorm = 0.5 * (3.0 - Renorm); //eq.21
+ Renorm = 0.5*(3.0 - Renorm); //eq.21
else
if ( (Renorm < 100.0) && (Renorm > 0.01) )
Renorm = 1.0 / sqrt( Renorm );
@@ -282,34 +350,32 @@
void DCMMotionCompensation(void) {
// compensation for rate of change of velocity LR/BF needs GPS velocity but
// updates probably too slow?
- Acc[LR ] += 0.0;
- Acc[BF] += 0.0;
+ AccV[LR ] += 0.0;
+ AccV[BF] += 0.0;
} // DCMMotionCompensation
void DCMDriftCorrection(void) {
- static real32 ScaledOmegaP[3], ScaledOmegaI[3];
- static real32 YawError[3];
- static real32 AccMagnitude, AccWeight;
- static real32 ErrorCourse;
-
- AccMagnitude = sqrt( Sqr(Acc[0]) + Sqr(Acc[1]) + Sqr(Acc[2]) );
+ static real32 ScaledOmegaI[3];
- // dynamic weighting of Accelerometer info (reliability filter)
- // weight for Accelerometer info ( < 0.5G = 0.0, 1G = 1.0 , > 1.5G = 0.0)
- AccWeight = Limit(1.0 - 2.0 * fabs(1.0 - AccMagnitude), 0.0, 1.0);
+ //DON'T USE #define USE_DCM_YAW_COMP
+#ifdef USE_DCM_YAW_COMP
+ static real32 ScaledOmegaP[3];
+ static real32 YawError[3];
+ static real32 ErrorCourse;
+#endif // USE_DCM_YAW_COMP
- VCross(&RollPitchError[0], &Acc[0], &DCM[2][0]); //adjust the reference ground
- VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch * AccWeight);
+ VCross(&RollPitchError[0], &AccV[0], &DCM[2][0]); //adjust the reference ground
+ VScale(&OmegaP[0], &RollPitchError[0], Kp_RollPitch);
- VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch * AccWeight);
+ VScale(&ScaledOmegaI[0], &RollPitchError[0], Ki_RollPitch);
VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
-#ifdef USE_DCM_YAW
+#ifdef USE_DCM_YAW_COMP
// Yaw - drift correction based on compass/magnetometer heading
- HeadingCos = cos( Heading );
- HeadingSin = sin( Heading );
- ErrorCourse = ( U[0][0] * HeadingSin ) - ( U[1][0] * HeadingCos );
+ HeadingCos = cos(Heading);
+ HeadingSin = sin(Heading);
+ ErrorCourse = ( U[0][0]*HeadingSin ) - ( U[1][0]*HeadingCos );
VScale(YawError, &U[2][0], ErrorCourse );
VScale(&ScaledOmegaP[0], &YawError[0], Kp_Yaw );
@@ -317,7 +383,8 @@
VScale(&ScaledOmegaI[0], &YawError[0], Ki_Yaw );
VAdd(&OmegaI[0], &OmegaI[0], &ScaledOmegaI[0]);
-#endif // USE_DCM_YAW
+#endif // USE_DCM_YAW_COMP
+
} // DCMDriftCorrection
void DCMUpdate(void) {
@@ -325,25 +392,29 @@
static uint8 i, j, k;
static real32 op[3];
+ AccV[BF] = Acc[BF];
+ AccV[LR] = -Acc[LR];
+ AccV[UD] = -Acc[UD];
+
VAdd(&Omega[0], &Gyro[0], &OmegaI[0]);
VAdd(&OmegaV[0], &Omega[0], &OmegaP[0]);
// MotionCompensation();
U[0][0] = 0.0;
- U[0][1] = -dT * OmegaV[2]; //-z
- U[0][2] = dT * OmegaV[1]; // y
- U[1][0] = dT * OmegaV[2]; // z
+ U[0][1] = -dT*OmegaV[2]; //-z
+ U[0][2] = dT*OmegaV[1]; // y
+ U[1][0] = dT*OmegaV[2]; // z
U[1][1] = 0.0;
- U[1][2] = -dT * OmegaV[0]; //-x
- U[2][0] = -dT * OmegaV[1]; //-y
- U[2][1] = dT * OmegaV[0]; // x
+ U[1][2] = -dT*OmegaV[0]; //-x
+ U[2][0] = -dT*OmegaV[1]; //-y
+ U[2][1] = dT*OmegaV[0]; // x
U[2][2] = 0.0;
for ( i = 0; i < (uint8)3; i++ )
for ( j = 0; j < (uint8)3; j++ ) {
for ( k = 0; k < (uint8)3; k++ )
- op[k] = DCM[i][k] * U[k][j];
+ op[k] = DCM[i][k]*U[k][j];
TempM[i][j] = op[0] + op[1] + op[2];
}
@@ -361,15 +432,21 @@
for ( g = 0; g < (uint8)3; g++ )
Rate[g] = Gyro[g];
- Angle[Pitch] = asin(DCM[2][0]);
- Angle[Roll] = -atan2(DCM[2][1], DCM[2][2]);
+ EstAngle[Roll][PremerlaniDCM]= atan2(DCM[2][1], DCM[2][2]);
+ EstAngle[Pitch][PremerlaniDCM] = -asin(DCM[2][0]);
+ EstAngle[Yaw][PremerlaniDCM] = atan2(DCM[1][0], DCM[0][0]);
-#ifdef USE_DCM_YAW
- Angle[Yaw] = atan2(DCM[1][0], DCM[0][0]);
-#endif // USE_DCM_YAW
+ // Est. Rates ???
} // DCMEulerAngles
+void DoDCM(void) {
+ DCMUpdate();
+ DCMNormalise();
+ DCMDriftCorrection();
+ DCMEulerAngles();
+} // DoDCM
+
//___________________________________________________________________________________
// IMU.c
@@ -380,80 +457,200 @@
// Quaternion implementation of the 'DCM filter' [Mahony et al.].
-// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
-
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation. See my report for an overview of the use of quaternions in this application.
// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')
-// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer
+// and accelerometer ('ax', 'ay', 'az') data. Gyroscope units are radians/second, accelerometer
// units are irrelevant as the vector is normalised.
-#include <math.h>
-
-void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az);
-
-#define MKp 2.0 // proportional gain governs rate of convergence to accelerometer/magnetometer
-#define MKi 0.005f // integral gain governs rate of convergence of gyroscope biases
+const real32 MKp = 2.0; // proportional gain governs rate of convergence to accelerometer/magnetometer
+const real32 MKi = 1.0; // integral gain governs rate of convergence of gyroscope biases // 0.005
+real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0; // scaled integral error
real32 q0 = 1.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; // quaternion elements representing the estimated orientation
-real32 exInt = 0.0, eyInt = 0.0, ezInt = 0.0; // scaled integral error
-void IMUupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {
+void DoMadgwickIMU(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az) {
- static real32 rnorm;
+ static uint8 g;
+ static real32 ReNorm;
static real32 vx, vy, vz;
static real32 ex, ey, ez;
- static real32 aMag;
+
+ if ( F.AccMagnitudeOK ) {
+
+ // estimated direction of gravity
+ vx = 2.0*(q1*q3 - q0*q2);
+ vy = 2.0*(q0*q1 + q2*q3);
+ vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);
+
+ // error is sum of cross product between reference direction of field and direction measured by sensor
+ ex = (ay*vz - az*vy);
+ ey = (az*vx - ax*vz);
+ ez = (ax*vy - ay*vx);
+
+ // integral error scaled integral gain
+ exInt += ex*MKi*dT;
+ eyInt += ey*MKi*dT;
+ ezInt += ez*MKi*dT;
-//swap z and y?
+ // adjusted gyroscope measurements
+ gx += MKp*ex + exInt;
+ gy += MKp*ey + eyInt;
+ gz += MKp*ez + ezInt;
+
+ // integrate quaternion rate and normalise
+ q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2;
+ q1 += (q0*gx + q2*gz - q3*gy)*dTOn2;
+ q2 += (q0*gy - q1*gz + q3*gx)*dTOn2;
+ q3 += (q0*gz + q1*gy - q2*gx)*dTOn2;
+
+ // normalise quaternion
+ ReNorm = 1.0 /sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
+ q0 *= ReNorm;
+ q1 *= ReNorm;
+ q2 *= ReNorm;
+ q3 *= ReNorm;
+
+ MadgwickEulerAngles(MadgwickIMU);
- // normalise the measurements
- aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero Acc values into AHRS is FATAL
- if ( aMag < 0.9 ) {
- ax = ay = 0.0;
- az = 1.0;
- } else {
- rnorm = 1.0/aMag;
- ax *= rnorm;
- ay *= rnorm;
- az *= rnorm;
- }
+ } else
+ for ( g = 0; g <(uint8)3; g++) {
+ EstRate[g][MadgwickIMU] = Gyro[g];
+ EstAngle[g][MadgwickIMU] += EstRate[g][MadgwickIMU]*dT;
+ }
+
+} // DoMadgwickIMU
+
+//_________________________________________________________________________________
+
+// IMU.c
+// S.O.H. Madgwick,
+// 'An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays',
+// April 30, 2010
+
+#ifdef INC_IMU2
+
+boolean FirstIMU2 = true;
+real32 BetaIMU2 = 0.033;
+// const real32 BetaAHRS = 0.041;
- // estimated direction of gravity
- vx = 2.0*(q1*q3 - q0*q2);
- vy = 2.0*(q0*q1 + q2*q3);
- vz = Sqr(q0) - Sqr(q1) - Sqr(q2) + Sqr(q3);
+//Quaternion orientation of earth frame relative to auxiliary frame.
+real32 AEq_1;
+real32 AEq_2;
+real32 AEq_3;
+real32 AEq_4;
+
+//Estimated orientation quaternion elements with initial conditions.
+real32 SEq_1;
+real32 SEq_2;
+real32 SEq_3;
+real32 SEq_4;
+
+void DoMadgwickIMU2(real32 w_x, real32 w_y, real32 w_z, real32 a_x, real32 a_y, real32 a_z) {
+
+ static uint8 g;
+
+ //Vector norm.
+ static real32 Renorm;
+ //Quaternion rate from gyroscope elements.
+ static real32 SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4;
+ //Objective function elements.
+ static real32 f_1, f_2, f_3;
+ //Objective function Jacobian elements.
+ static real32 J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
+ //Objective function gradient elements.
+ static real32 SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;
- // error is sum of cross product between reference direction of field and direction measured by sensor
- ex = (ay*vz - az*vy);
- ey = (az*vx - ax*vz);
- ez = (ax*vy - ay*vx);
+ //Auxiliary variables to avoid reapeated calcualtions.
+ static real32 halfSEq_1, halfSEq_2, halfSEq_3, halfSEq_4;
+ static real32 twoSEq_1, twoSEq_2, twoSEq_3;
+
+ if ( F.AccMagnitudeOK ) {
+
+ halfSEq_1 = 0.5*SEq_1;
+ halfSEq_2 = 0.5*SEq_2;
+ halfSEq_3 = 0.5*SEq_3;
+ halfSEq_4 = 0.5*SEq_4;
+ twoSEq_1 = 2.0*SEq_1;
+ twoSEq_2 = 2.0*SEq_2;
+ twoSEq_3 = 2.0*SEq_3;
+
+ //Compute the quaternion rate measured by gyroscopes.
+ SEqDot_omega_1 = -halfSEq_2*w_x - halfSEq_3*w_y - halfSEq_4*w_z;
+ SEqDot_omega_2 = halfSEq_1*w_x + halfSEq_3*w_z - halfSEq_4*w_y;
+ SEqDot_omega_3 = halfSEq_1*w_y - halfSEq_2*w_z + halfSEq_4*w_x;
+ SEqDot_omega_4 = halfSEq_1*w_z + halfSEq_2*w_y - halfSEq_3*w_x;
- // integral error scaled integral gain
- exInt += ex*MKi;
- eyInt += ey*MKi;
- ezInt += ez*MKi;
+ /*
+ //Normalise the accelerometer measurement.
+ Renorm = 1.0 / sqrt(Sqr(a_x) + Sqr(a_y) + Sqr(a_z));
+ a_x *= Renorm;
+ a_y *= Renorm;
+ a_z *= Renorm;
+ */
- // adjusted gyroscope measurements
- gx += MKp*ex + exInt;
- gy += MKp*ey + eyInt;
- gz += MKp*ez + ezInt;
+ //Compute the objective function and Jacobian.
+ f_1 = twoSEq_2*SEq_4 - twoSEq_1*SEq_3 - a_x;
+ f_2 = twoSEq_1*SEq_2 + twoSEq_3*SEq_4 - a_y;
+ f_3 = 1.0 - twoSEq_2*SEq_2 - twoSEq_3*SEq_3 - a_z;
+ //J_11 negated in matrix multiplication.
+ J_11or24 = twoSEq_3;
+ J_12or23 = 2.0*SEq_4;
+ //J_12 negated in matrix multiplication
+ J_13or22 = twoSEq_1;
+ J_14or21 = twoSEq_2;
+ //Negated in matrix multiplication.
+ J_32 = 2.0*J_14or21;
+ //Negated in matrix multiplication.
+ J_33 = 2.0*J_11or24;
- // integrate quaternion rate and normalise
- q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
- q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
- q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
- q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;
+ //Compute the gradient (matrix multiplication).
+ SEqHatDot_1 = J_14or21*f_2 - J_11or24*f_1;
+ SEqHatDot_2 = J_12or23*f_1 + J_13or22*f_2 - J_32*f_3;
+ SEqHatDot_3 = J_12or23*f_2 - J_33*f_3 - J_13or22*f_1;
+ SEqHatDot_4 = J_14or21*f_1 + J_11or24*f_2;
+
+ //Normalise the gradient.
+ Renorm = 1.0 / sqrt(Sqr(SEqHatDot_1) + Sqr(SEqHatDot_2) + Sqr(SEqHatDot_3) + Sqr(SEqHatDot_4));
+ SEqHatDot_1 *= Renorm;
+ SEqHatDot_2 *= Renorm;
+ SEqHatDot_3 *= Renorm;
+ SEqHatDot_4 *= Renorm;
+
+ //Compute then integrate the estimated quaternion rate.
+ SEq_1 += (SEqDot_omega_1 - (BetaIMU2*SEqHatDot_1))*dT;
+ SEq_2 += (SEqDot_omega_2 - (BetaIMU2*SEqHatDot_2))*dT;
+ SEq_3 += (SEqDot_omega_3 - (BetaIMU2*SEqHatDot_3))*dT;
+ SEq_4 += (SEqDot_omega_4 - (BetaIMU2*SEqHatDot_4))*dT;
- // normalise quaternion
- rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
- q0 *= rnorm;
- q1 *= rnorm;
- q2 *= rnorm;
- q3 *= rnorm;
+ //Normalise quaternion
+ Renorm = 1.0 / sqrt(Sqr(SEq_1) + Sqr(SEq_2) + Sqr(SEq_3) + Sqr(SEq_4));
+ SEq_1 *= Renorm;
+ SEq_2 *= Renorm;
+ SEq_3 *= Renorm;
+ SEq_4 *= Renorm;
-} // IMUupdate
+ if ( FirstIMU2 ) {
+ //Store orientation of auxiliary frame.
+ AEq_1 = SEq_1;
+ AEq_2 = SEq_2;
+ AEq_3 = SEq_3;
+ AEq_4 = SEq_4;
+ FirstIMU2 = false;
+ }
+
+ MadgwickEulerAngles(MadgwickIMU2);
+
+ } else
+ for ( g = 0; g <(uint8)3; g++) {
+ EstRate[g][MadgwickIMU2] = Gyro[g];
+ EstAngle[g][MadgwickIMU2] += EstRate[g][MadgwickIMU2]*dT;
+ }
+
+} // DoMadgwickIMU2
+
+#endif // INC_IMU2
//_________________________________________________________________________________
@@ -466,137 +663,332 @@
// Quaternion implementation of the 'DCM filter' [Mahoney et al]. Incorporates the magnetic distortion
// compensation algorithms from my filter [Madgwick] which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
-// axis only.
+// a only.
-// User must define 'HalfdT' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
+// User must define 'dTOn2' as the (sample period / 2), and the filter gains 'MKp' and 'MKi'.
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation. See my report for an overview of the use of quaternions in this application.
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
-// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are
+// accelerometer ('ax', 'ay', 'az') and magnetometer ('mx', 'my', 'mz') data. Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
-void AHRSupdate(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {
+void DoMadgwickAHRS(real32 gx, real32 gy, real32 gz, real32 ax, real32 ay, real32 az, real32 mx, real32 my, real32 mz) {
- static real32 rnorm;
+ static uint8 g;
+ static real32 ReNorm;
static real32 hx, hy, hz, bx2, bz2, mx2, my2, mz2;
static real32 vx, vy, vz, wx, wy, wz;
static real32 ex, ey, ez;
static real32 q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
- static real32 aMag;
+
+ if ( F.AccMagnitudeOK ) {
+
+ // auxiliary variables to reduce number of repeated operations
+ q0q0 = q0*q0;
+ q0q1 = q0*q1;
+ q0q2 = q0*q2;
+ q0q3 = q0*q3;
+ q1q1 = q1*q1;
+ q1q2 = q1*q2;
+ q1q3 = q1*q3;
+ q2q2 = q2*q2;
+ q2q3 = q2*q3;
+ q3q3 = q3*q3;
+
+ ReNorm = 1.0 / sqrt( Sqr( mx ) + Sqr( my ) + Sqr( mz ) );
+ mx *= ReNorm;
+ my *= ReNorm;
+ mz *= ReNorm;
+ mx2 = 2.0*mx;
+ my2 = 2.0*my;
+ mz2 = 2.0*mz;
+
+ // compute reference direction of flux
+ hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
+ hy = mx2*(q1q2 + q0q3) + my2*( 0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
+ hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*( 0.5 - q1q1 - q2q2 );
+ bx2 = 2.0*sqrt( Sqr( hx ) + Sqr( hy ) );
+ bz2 = 2.0*hz;
+
+ // estimated direction of gravity and flux (v and w)
+ vx = 2.0*(q1q3 - q0q2);
+ vy = 2.0*(q0q1 + q2q3);
+ vz = q0q0 - q1q1 - q2q2 + q3q3;
+
+ wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
+ wy = bx2*(q1q2 - q0q3) + bz2*( q0q1 + q2q3 );
+ wz = bx2*(q0q2 + q1q3) + bz2*( 0.5 - q1q1 - q2q2 );
+
+ // error is sum of cross product between reference direction of fields and direction measured by sensors
+ ex = (ay*vz - az*vy) + (my*wz - mz*wy);
+ ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
+ ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
+
+ // integral error scaled integral gain
+ exInt += ex*MKi*dT;
+ eyInt += ey*MKi*dT;
+ ezInt += ez*MKi*dT;
+
+ // adjusted gyroscope measurements
+ gx += MKp*ex + exInt;
+ gy += MKp*ey + eyInt;
+ gz += MKp*ez + ezInt;
+
+ // integrate quaternion rate and normalise
+ q0 += (-q1*gx - q2*gy - q3*gz)*dTOn2;
+ q1 += (q0*gx + q2*gz - q3*gy)*dTOn2;
+ q2 += (q0*gy - q1*gz + q3*gx)*dTOn2;
+ q3 += (q0*gz + q1*gy - q2*gx)*dTOn2;
+
+ // normalise quaternion
+ ReNorm = 1.0 / sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
+ q0 *= ReNorm;
+ q1 *= ReNorm;
+ q2 *= ReNorm;
+ q3 *= ReNorm;
+
+ MadgwickEulerAngles(MadgwickAHRS);
+
+ } else
+ for ( g = 0; g <(uint8)3; g++) {
+ EstRate[g][MadgwickAHRS] = Gyro[g];
+ EstAngle[g][MadgwickAHRS] += EstRate[g][MadgwickAHRS]*dT;
+ }
+
+} // DoMadgwickAHRS
+
+void MadgwickEulerAngles(uint8 S) {
+
+ EstAngle[Roll][S] = atan2(2.0*q2*q3 - 2.0*q0*q1, 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
+ EstAngle[Pitch][S] = asin(2.0*q1*q2 - 2.0*q0*q2);
+ EstAngle[Yaw][S] = atan2(2.0*q1*q2 - 2.0*q0*q3, 2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
+
+} // MadgwickEulerAngles
+
+//_________________________________________________________________________________
+
+// Kalman Filter originally authored by Tom Pycke
+// http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data
- // auxiliary variables to reduce number of repeated operations
- q0q0 = q0*q0;
- q0q1 = q0*q1;
- q0q2 = q0*q2;
- q0q3 = q0*q3;
- q1q1 = q1*q1;
- q1q2 = q1*q2;
- q1q3 = q1*q3;
- q2q2 = q2*q2;
- q2q3 = q2*q3;
- q3q3 = q3*q3;
+real32 AngleKF[2] = {0,0};
+real32 BiasKF[2] = {0,0};
+real32 P00[2] = {0,0};
+real32 P01[2] = {0,0};
+real32 P10[2] = {0,0};
+real32 P11[2] = {0,0};
+
+real32 KalmanFilter(uint8 a, real32 NewAngle, real32 NewRate) {
+
+ // Q is a 2x2 matrix that represents the process covariance noise.
+ // In this case, it indicates how much we trust the accelerometer
+ // relative to the gyros.
+ const real32 AngleQ = 0.003;
+ const real32 GyroQ = 0.009;
+
+ // R represents the measurement covariance noise. In this case,
+ // it is a 1x1 matrix that says that we expect AngleR rad jitter
+ // from the accelerometer.
+ const real32 AngleR = GYRO_PROP_NOISE;
+
+ static real32 y, S;
+ static real32 K0, K1;
+
+ AngleKF[a] += (NewRate - BiasKF[a])*dT;
+ P00[a] -= (( P10[a] + P01[a] ) + AngleQ )*dT;
+ P01[a] -= P11[a]*dT;
+ P10[a] -= P11[a]*dT;
+ P11[a] += GyroQ*dT;
+
+ y = NewAngle - AngleKF[a];
+ S = 1.0 / ( P00[a] + AngleR );
+ K0 = P00[a]*S;
+ K1 = P10[a]*S;
+
+ AngleKF[a] += K0*y;
+ BiasKF[a] += K1*y;
+ P00[a] -= K0*P00[a];
+ P01[a] -= K0*P01[a];
+ P10[a] -= K1*P00[a];
+ P11[a] -= K1*P01[a];
+
+ return ( AngleKF[a] );
+
+} // KalmanFilter
- aMag = sqrt(Sqr(ax) + Sqr(ay) + Sqr(az)); // zero values into AHRS is FATAL
- if ( aMag < 0.9 ) {
- ax = ay = 0.0;
- az = 1.0;
- } else {
- // normalise the measurements
- rnorm = 1.0/aMag;
- ax *= rnorm;
- ay *= rnorm;
- az *= rnorm;
+void DoKalman(void) { // NO YAW ANGLE ESTIMATE
+ EstAngle[Roll][Kalman] = KalmanFilter(Roll, asin(-Acc[LR]), Gyro[Roll]);
+ EstAngle[Pitch][Kalman] = KalmanFilter(Pitch, asin(Acc[BF]), Gyro[Pitch]);
+ EstRate[Roll][Kalman] = Gyro[Roll];
+ EstRate[Pitch][Kalman] = Gyro[Pitch];
+} // DoKalman
+
+
+//_________________________________________________________________________________
+
+// MultWii
+// Original code by Alex at: http://radio-commande.com/international/triwiicopter-design/
+// simplified IMU based on Kalman Filter
+// inspired from http://starlino.com/imu_guide.html
+// and http://www.starlino.com/imu_kalman_arduino.html
+// with this algorithm, we can get absolute angles for a stable mode integration
+
+real32 AccMW[3] = {0.0, 0.0, 1.0}; // init acc in stable mode
+real32 GyroMW[3] = {0.0, 0.0, 0.0}; // R obtained from last estimated value and gyro movement
+real32 Axz, Ayz; // angles between projection of R on XZ/YZ plane and Z axis
+
+void DoMultiWii(void) { // V1.6 NO YAW ANGLE ESTIMATE
+
+ const real32 GyroWt = 50.0; // gyro weight/smoothing factor
+ const real32 GyroWtR = 1.0 / GyroWt;
+
+ if ( Acc[UD] < 0.0 ) { // check not inverted
+
+ if ( F.AccMagnitudeOK ) {
+ Ayz = atan2( AccMW[LR], AccMW[UD] ) + Gyro[Roll]*dT;
+ Axz = atan2( AccMW[BF], AccMW[UD] ) + Gyro[Pitch]*dT;
+ } else {
+ Ayz += Gyro[Roll]*dT;
+ Axz += Gyro[Pitch]*dT;
+ }
+
+ // reverse calculation of GyroMW from Awz angles,
+ // for formulae deduction see http://starlino.com/imu_guide.html
+ GyroMW[Roll] = sin( Ayz ) / sqrt( 1.0 + Sqr( cos( Ayz ) )*Sqr( tan( Axz ) ) );
+ GyroMW[Pitch] = sin( Axz ) / sqrt( 1.0 + Sqr( cos( Axz ) )*Sqr( tan( Ayz ) ) );
+ GyroMW[Yaw] = sqrt( fabs( 1.0 - Sqr( GyroMW[Roll] ) - Sqr( GyroMW[Pitch] ) ) );
+
+ //combine accelerometer and gyro readings
+ AccMW[LR] = ( -Acc[LR] + GyroWt*GyroMW[Roll] )*GyroWtR;
+ AccMW[BF] = ( Acc[BF] + GyroWt*GyroMW[Pitch] )*GyroWtR;
+ AccMW[UD] = ( -Acc[UD] + GyroWt*GyroMW[Yaw] )*GyroWtR;
}
- rnorm = 1.0/sqrt(Sqr(mx) + Sqr(my) + Sqr(mz));
- mx *= rnorm;
- my *= rnorm;
- mz *= rnorm;
- mx2 = 2.0 * mx;
- my2 = 2.0 * my;
- mz2 = 2.0 * mz;
+ EstAngle[Roll][MultiWii] = Ayz;
+ EstAngle[Pitch][MultiWii] = Axz;
- // compute reference direction of flux
- hx = mx2*(0.5 - q2q2 - q3q3) + my2*(q1q2 - q0q3) + mz2*(q1q3 + q0q2);
- hy = mx2*(q1q2 + q0q3) + my2*(0.5 - q1q1 - q3q3) + mz2*(q2q3 - q0q1);
- hz = mx2*(q1q3 - q0q2) + my2*(q2q3 + q0q1) + mz2*(0.5 - q1q1 - q2q2);
- bx2 = 2.0*sqrt(Sqr(hx) + Sqr(hy));
- bz2 = 2.0*hz;
+// EstRate[Roll][MultiWii] = GyroMW[Roll];
+// EstRate[Pitch][MultiWii] = GyroMW[Pitch];
+
+ EstRate[Roll][MultiWii] = Gyro[Roll];
+ EstRate[Pitch][MultiWii] = Gyro[Pitch];
+
+} // DoMultiWii
- // estimated direction of gravity and flux (v and w)
- vx = 2.0*(q1q3 - q0q2);
- vy = 2.0*(q0q1 + q2q3);
- vz = q0q0 - q1q1 - q2q2 + q3q3;
+//_________________________________________________________________________________
- wx = bx2*(0.5 - q2q2 - q3q3) + bz2*(q1q3 - q0q2);
- wy = bx2*(q1q2 - q0q3) + bz2*(q0q1 + q2q3);
- wz = bx2*(q0q2 + q1q3) + bz2*(0.5 - q1q1 - q2q2);
+void AttitudeTest(void) {
+
+ TxString("\r\nAttitude Test\r\n");
- // error is sum of cross product between reference direction of fields and direction measured by sensors
- ex = (ay*vz - az*vy) + (my*wz - mz*wy);
- ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
- ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
+ GetAttitude();
- // integral error scaled integral gain
- exInt += ex*MKi;
- eyInt += ey*MKi;
- ezInt += ez*MKi;
+ TxString("\r\ndT \t");
+ TxVal32(dT*1000.0, 3, 0);
+ TxString(" Sec.\r\n\r\n");
+
+ if ( GyroType == IRSensors ) {
- // adjusted gyroscope measurements
- gx += MKp*ex + exInt;
- gy += MKp*ey + eyInt;
- gz += MKp*ez + ezInt;
+ TxString("IR Sensors:\r\n");
+ TxString("\tRoll \t");
+ TxVal32(IR[Roll]*100.0, 2, HT);
+ TxNextLine();
+ TxString("\tPitch\t");
+ TxVal32(IR[Pitch]*100.0, 2, HT);
+ TxNextLine();
+ TxString("\tZ \t");
+ TxVal32(IR[Yaw]*100.0, 2, HT);
+ TxNextLine();
+ TxString("\tMin/Max\t");
+ TxVal32(IRMin*100.0, 2, HT);
+ TxVal32(IRMax*100.0, 2, HT);
+ TxString("\tSwing\t");
+ TxVal32(IRSwing*100.0, 2, HT);
+ TxNextLine();
- // integrate quaternion rate and normalise
- q0 += (-q1*gx - q2*gy - q3*gz)*HalfdT;
- q1 += (q0*gx + q2*gz - q3*gy)*HalfdT;
- q2 += (q0*gy - q1*gz + q3*gx)*HalfdT;
- q3 += (q0*gz + q1*gy - q2*gx)*HalfdT;
-
- // normalise quaternion
- rnorm = 1.0/sqrt(Sqr(q0) + Sqr(q1) + Sqr(q2) + Sqr(q3));
- q0 *= rnorm;
- q1 *= rnorm;
- q2 *= rnorm;
- q3 *= rnorm;
+ } else {
-} // AHRSupdate
-
-void EulerAngles(void) {
-
- static uint8 g;
+ TxString("Gyro, Compensated, Max Delta(Deg./Sec.):\r\n");
+ TxString("\tRoll \t");
+ TxVal32(Gyro[Roll]*MILLIANGLE, 3, HT);
+ TxVal32(Rate[Roll]*MILLIANGLE, 3, HT);
+ TxVal32(GyroNoise[Roll]*MILLIANGLE,3, 0);
+ TxNextLine();
+ TxString("\tPitch\t");
+ TxVal32(Gyro[Pitch]*MILLIANGLE, 3, HT);
+ TxVal32(Rate[Pitch]*MILLIANGLE, 3, HT);
+ TxVal32(GyroNoise[Pitch]*MILLIANGLE,3, 0);
+ TxNextLine();
+ TxString("\tYaw \t");
+ TxVal32(Gyro[Yaw]*MILLIANGLE, 3, HT);
+ TxVal32(Rate[Yaw]*MILLIANGLE, 3, HT);
+ TxVal32(GyroNoise[Yaw]*MILLIANGLE, 3, 0);
+ TxNextLine();
- Angle[Roll] = atan2(2.0*q2*q3 - 2.0*q0*q1 , 2.0*Sqr(q0) + 2.0*Sqr(q3) - 1.0);
- Angle[Pitch] = asin(2.0*q1*q2 - 2.0*q0*q2);
- Angle[Yaw] = -atan2(2.0*q1*q2 - 2.0*q0*q3 , 2.0*Sqr(q0) + 2.0*Sqr(q1) - 1.0);
-
- for ( g = 0; g < (uint8)3; g++ ) {
-#ifdef USE_GYRO_RATE
- Rate[g] = Gyro[g];
-#else
- Rate[g] = ( Angle[g] - Anglep[g] ) * dTR;
- Anglep[g] = Angle[g];
-#endif // USE_GYRO_RATE
+ TxString("Accelerations , peak change(G):\r\n");
+ TxString("\tB->F \t");
+ TxVal32(Acc[BF]*1000.0, 3, HT);
+ TxVal32( AccNoise[BF]*1000.0, 3, 0);
+ TxNextLine();
+ TxString("\tL->R \t");
+ TxVal32(Acc[LR]*1000.0, 3, HT);
+ TxVal32( AccNoise[LR]*1000.0, 3, 0);
+ TxNextLine();
+ TxString("\tU->D \t");
+ TxVal32(Acc[UD]*1000.0, 3, HT);
+ TxVal32( AccNoise[UD]*1000.0, 3, 0);
+ TxNextLine();
}
-} // EulerAngles
+ if ( CompassType != HMC6352 ) {
+ TxString("Magnetic:\r\n");
+ TxString("\tX \t");
+ TxVal32(Mag[Roll].V, 0, 0);
+ TxNextLine();
+ TxString("\tY \t");
+ TxVal32(Mag[Pitch].V, 0, 0);
+ TxNextLine();
+ TxString("\tZ \t");
+ TxVal32(Mag[Yaw].V, 0, 0);
+ TxNextLine();
+ }
-/*
-heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
-attitude = asin(2*qx*qy + 2*qz*qw)
-bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)
+ TxString("Heading: \t");
+ TxVal32(Make2Pi(Heading)*MILLIANGLE, 3, 0);
+ TxNextLine();
+
+} // AttitudeTest
+
+void InitAttitude(void) {
+
+ static uint8 a, s;
+
+#ifdef INC_IMU2
-except when qx*qy + qz*qw = 0.5 (north pole)
-which gives:
-heading = 2 * atan2(x,w)
-bank = 0
-and when qx*qy + qz*qw = -0.5 (south pole)
-which gives:
-heading = -2 * atan2(x,w)
-bank = 0
-*/
+ FirstIMU2 = true;
+ BetaIMU2 = sqrt(0.75) * GyroNoiseRadian[GyroType];
+
+ //Quaternion orientation of earth frame relative to auxiliary frame.
+ AEq_1 = 1.0;
+ AEq_2 = 0.0;
+ AEq_3 = 0.0;
+ AEq_4 = 0.0;
+ //Estimated orientation quaternion elements with initial conditions.
+ SEq_1 = 1.0;
+ SEq_2 = 0.0;
+ SEq_3 = 0.0;
+ SEq_4 = 0.0;
+#endif // INC_IMU2
+ for ( a = 0; a < (uint8)2; a++ )
+ AngleCF[a] = AngleKF[a] = BiasKF[a] = F0[a] = F1[a] = F2[a] = 0.0;
+
+ for ( a = 0; a < (uint8)3; a++ )
+ for ( s = 0; s < MaxAttitudeScheme; s++ )
+ EstAngle[a][s] = EstRate[a][s] = 0.0;
+
+} // InitAttitude
+