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

AHRS: Export the bias drift corrected gyro values to OP so stabilization see

Browse Source 
more accurate roll rates

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2174 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-11-29 08:44:41 +00:00 committed by peabody124
parent 0cae6df8b0
commit f51a5a824e
3 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/AHRS/ahrs.c
View File

@ -635,9 +635,9 @@ void downsample_data()
raw.gyrotemp[0] = valid_data_buffer[6];
raw.gyrotemp[1] = valid_data_buffer[7];
raw.gyros_filtered[0] = gyro_data.filtered.x * 180 / M_PI;
raw.gyros_filtered[1] = gyro_data.filtered.y * 180 / M_PI;
raw.gyros_filtered[2] = gyro_data.filtered.z * 180 / M_PI;
raw.gyros_filtered[0] = (gyro_data.filtered.x - Nav.gyro_bias[0]) * 180 / M_PI;
raw.gyros_filtered[1] = (gyro_data.filtered.y - Nav.gyro_bias[1]) * 180 / M_PI;
raw.gyros_filtered[2] = (gyro_data.filtered.z - Nav.gyro_bias[2]) * 180 / M_PI;
raw.accels[0] = valid_data_buffer[2];
raw.accels[1] = valid_data_buffer[0];

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

@ -79,6 +79,7 @@ struct NavStruct {
float Pos[3]; // Position in meters and relative to a local NED frame
float Vel[3]; // Velocity in meters and in NED
float q[4]; // unit quaternion rotation relative to NED
float gyro_bias[3];
} Nav;
/**

5
flight/AHRS/insgps.c
View File

@ -92,7 +92,7 @@ void INSGPSInit() //pretty much just a place holder for now
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-6; // gyro bias random walk variance (rad/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-14; // gyro bias random walk variance (rad/s^2)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
@ -234,6 +234,9 @@ void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
Nav.q[1] = X[7];
Nav.q[2] = X[8];
Nav.q[3] = X[9];
Nav.gyro_bias[0] = X[10];
Nav.gyro_bias[1] = X[11];
Nav.gyro_bias[2] = X[12];
}
void INSCovariancePrediction(float dT)