rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Flight/AHRS: Reorganized the code a little to make sure indoor used by default unless GPS updates come

Browse Source 
in.  Also change the velocity variance in indoor mode to lower to make it get a better attitude estimate (primarily faster convergence - probably will tweak this in the future dynamically).  Finally decreased barometer variance - in scope it's actually quite reasonable, normally within +/- .5 m.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1546 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-09-06 07:54:19 +00:00 committed by peabody124
parent e260077247
commit e06162fa3a
2 changed files with 21 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
flight/AHRS/ahrs.c
View File

@ -225,6 +225,7 @@ int main()
{
float gyro[3], accel[3], mag[3];
float vel[3] = {0,0,0};
gps_data.quality = -1;
ahrs_algorithm = INSGPS_Algo;
@ -356,35 +357,29 @@ int main()
INSPrediction(gyro, accel, 1 / (float) EKF_RATE);
if ( gps_data.updated )
if ( gps_data.updated && gps_data.quality == 1)
{
if(gps_data.quality == 1)
{
// Compute velocity from Heading and groundspeed
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
// Completely unprincipled way to make the position variance
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar(0.004);
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
gps_data.updated = false;
} else if(gps_data.quality == 0) {
// Not indoors but lost lock.
// TODO: Want to clamp coviance matrix to initial conditions on diagonal
MagCorrection(mag);
} else if(gps_data.quality == -1) {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(10);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection(vel,altitude_data.altitude);
}
// Compute velocity from Heading and groundspeed
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180);
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180);
// Completely unprincipled way to make the position variance
// increase as data quality decreases but keep it bounded
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
INSSetPosVelVar(0.004);
FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude);
gps_data.updated = false;
}
else if(gps_data.quality != -1)
MagCorrection(mag); // only trust mags if outdoors
else {
// Indoors, update with zero position and velocity and high covariance
INSSetPosVelVar(0.1);
vel[0] = 0;
vel[1] = 0;
vel[2] = 0;
VelBaroCorrection(vel,altitude_data.altitude);
}
attitude_data.quaternion.q1 = Nav.q[0];
attitude_data.quaternion.q2 = Nav.q[1];

2
flight/AHRS/insgps.c
View File

@ -82,7 +82,7 @@ void INSGPSInit() //pretty much just a place holder for now
R[3]=R[4]=0.004; // High freq GPS horizontal velocity noise variance (m/s)^2
R[5]=100; // High freq GPS vertical velocity noise variance (m/s)^2
R[6]=R[7]=R[8]=0.005; // magnetometer unit vector noise variance
R[9]=1; // High freq altimeter noise variance (m^2)
R[9]=.05; // High freq altimeter noise variance (m^2)
}
void INSSetPosVelVar(float PosVar)