rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00
Code Issues Releases Activity

OP-235 AHRS: Not sending the out of range values to the mainboard was blocking

Browse Source 
calibration.  Now only don't send them to the EKF.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2342 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2011-01-06 16:44:40 +00:00 committed by peabody124
parent 050ade4899
commit ccc8bc089b
1 changed files with 13 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
flight/AHRS/ahrs.c
View File

@ -653,15 +653,6 @@ void adc_callback(float * downsampled_data)
gyro[0] = ( ( downsampled_data[1] + gyro_data.calibration.tempcompfactor[0] * downsampled_data[6] ) * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
gyro[1] = ( ( downsampled_data[3] + gyro_data.calibration.tempcompfactor[1] * downsampled_data[6] ) * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
gyro[2] = ( ( downsampled_data[5] + gyro_data.calibration.tempcompfactor[2] * downsampled_data[7] ) * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
if(isnan(accel[0] + accel[1] + accel[2]) ||
isnan(gyro[0] + gyro[1] + gyro[2]) ||
ACCEL_OOB(accel[0] + accel[1] + accel[2]) ||
GYRO_OOB(gyro[0] + gyro[1] + gyro[2])) {
// If any values are NaN or inf don't push them
bad_values++;
return;
}
#if 0
static float gravity_tracking[3] = {0,0,0};
@ -676,13 +667,21 @@ void adc_callback(float * downsampled_data)
accel[2] -= gravity_tracking[2];
}
#endif
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
if(!(isnan(accel[0] + accel[1] + accel[2]) ||
isnan(gyro[0] + gyro[1] + gyro[2]) ||
ACCEL_OOB(accel[0] + accel[1] + accel[2]) ||
GYRO_OOB(gyro[0] + gyro[1] + gyro[2]))) {
if(fifoBuf_getFree(&adc_fifo_buffer) >= (sizeof(accel) + sizeof(gyro))) {
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &accel[0], sizeof(accel));
fifoBuf_putData(&adc_fifo_buffer, (uint8_t *) &gyro[0], sizeof(gyro));
} else {
ekf_too_slow++;
}
} else {
ekf_too_slow++;
// If any values are NaN or inf don't push them
bad_values++;
}
AttitudeRawData raw;
raw.accels[0] = downsampled_data[2];