rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-27 16:54:15 +01:00
Code Issues Releases Activity

AHRS: Separate covariance prediction from state prediction to get updated state out of spi a few milliseconds sooner.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1675 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-17 18:15:00 +00:00 committed by peabody124
parent 92c36114f3
commit 0ea95fbad1
3 changed files with 18 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/AHRS/ahrs.c
View File

@ -361,8 +361,10 @@ int main()
mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
INSStatePrediction(gyro, accel, 1 / (float) EKF_RATE);
process_spi_request();
INSCovariancePrediction(1 / (float) EKF_RATE);
if ( gps_data.updated && gps_data.quality == 1)
{
// Compute velocity from Heading and groundspeed
@ -418,6 +420,8 @@ int main()
attitude_data.quaternion.q2 = q[1];
attitude_data.quaternion.q3 = q[2];
attitude_data.quaternion.q4 = q[3];
process_spi_request();
}
ahrs_state = AHRS_IDLE;
@ -455,7 +459,6 @@ int main()
}
#endif
process_spi_request();
}
return 0;
@ -644,7 +647,8 @@ void converge_insgps()
mag[2] = -mag_data.raw.axis[2];
RPY2Quaternion(rpy,Nav.q);
INSPrediction( temp_gyro, accel, 1 / (float) EKF_RATE );
INSStatePrediction( temp_gyro, accel, 1 / (float) EKF_RATE );
INSCovariancePrediction(1 / (float) EKF_RATE);
FullCorrection(mag,pos,vel,BaroAlt);
process_spi_request(); // again we must keep this hear to prevent SPI connection dropping
}

4
flight/AHRS/inc/insgps.h
View File

@ -50,7 +50,9 @@
// Exposed Function Prototypes
void INSGPSInit();
void INSPrediction(float gyro_data[3], float accel_data[3], float dT);
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSSetPosVelVar(float PosVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);

9
flight/AHRS/insgps.c
View File

@ -136,7 +136,7 @@ void INSSetMagNorth(float B[3])
Be[2] = B[2];
}
void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
{
float U[6];
float qmag;
@ -156,7 +156,7 @@ void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
RungeKutta(X,U,dT);
qmag=sqrt(X[6]*X[6] + X[7]*X[7] + X[8]*X[8] + X[9]*X[9]);
X[6] /= qmag; X[7] /= qmag; X[8] /= qmag; X[9] /= qmag;
CovariancePrediction(F,G,Q,dT,P);
//CovariancePrediction(F,G,Q,dT,P);
// Update Nav solution structure
Nav.Pos[0] = X[0];
@ -171,6 +171,11 @@ void INSPrediction(float gyro_data[3], float accel_data[3], float dT)
Nav.q[3] = X[9];
}
void INSCovariancePrediction(float dT)
{
CovariancePrediction(F,G,Q,dT,P);
}
float zeros[3] = {0,0,0};
void MagCorrection(float mag_data[3])