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

INS: Store the gyro bias in the settings so it doesn't get overwritten

Browse Source
This commit is contained in:
James Cotton 2011-09-04 15:21:53 -05:00
parent c326c0941d
commit cd8ebdf427
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/INS/ins.c
View File

@ -73,7 +73,8 @@ void get_accel_gyro_data();
void reset_values(); void reset_values();
void measure_noise(void); void measure_noise(void);
void zero_gyros(); void zero_gyros(bool update_settings);
/* Communication functions */ /* Communication functions */
//void send_calibration(void); //void send_calibration(void);
void send_attitude(void); void send_attitude(void);
@ -237,7 +238,7 @@ int main()
} }
if(total_conversion_blocks <= 3000 && !zeroed_gyros) { if(total_conversion_blocks <= 3000 && !zeroed_gyros) {
zero_gyros(); zero_gyros(total_conversion_blocks == 3000);
if(total_conversion_blocks == 3000) if(total_conversion_blocks == 3000)
zeroed_gyros = true; zeroed_gyros = true;
PIOS_DELAY_WaituS(TYPICAL_PERIOD); PIOS_DELAY_WaituS(TYPICAL_PERIOD);
@ -274,7 +275,7 @@ int main()
while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
break; break;
case INSSETTINGS_ALGORITHM_ZEROGYROS: case INSSETTINGS_ALGORITHM_ZEROGYROS:
zero_gyros(); zero_gyros(false);
// Run at standard rate // Run at standard rate
while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
break; break;
@ -581,12 +582,21 @@ void measure_noise()
} }
} }
void zero_gyros() void zero_gyros(bool update_settings)
{ {
const float rate = 1e-2; const float rate = 1e-2;
gyro_data.calibration.bias[0] += -gyro_data.filtered.x * rate; gyro_data.calibration.bias[0] += -gyro_data.filtered.x * rate;
gyro_data.calibration.bias[1] += -gyro_data.filtered.y * rate; gyro_data.calibration.bias[1] += -gyro_data.filtered.y * rate;
gyro_data.calibration.bias[2] += -gyro_data.filtered.z * rate; gyro_data.calibration.bias[2] += -gyro_data.filtered.z * rate;
if(update_settings) {
InsSettingsData settings;
InsSettingsGet(&settings);
settings.gyro_bias[INSSETTINGS_GYRO_BIAS_X] = gyro_data.calibration.bias[0];
settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Y] = gyro_data.calibration.bias[1];
settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Z] = gyro_data.calibration.bias[2];
InsSettingsSet(&settings);
}
} }
/** /**