From cd8ebdf427bb1b5cefaded10456d41909a279799 Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sun, 4 Sep 2011 15:21:53 -0500 Subject: [PATCH] INS: Store the gyro bias in the settings so it doesn't get overwritten --- flight/INS/ins.c | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/flight/INS/ins.c b/flight/INS/ins.c index ca4e6229f..2ecfeb88b 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -73,7 +73,8 @@ void get_accel_gyro_data(); void reset_values(); void measure_noise(void); -void zero_gyros(); +void zero_gyros(bool update_settings); + /* Communication functions */ //void send_calibration(void); void send_attitude(void); @@ -237,7 +238,7 @@ int main() } if(total_conversion_blocks <= 3000 && !zeroed_gyros) { - zero_gyros(); + zero_gyros(total_conversion_blocks == 3000); if(total_conversion_blocks == 3000) zeroed_gyros = true; PIOS_DELAY_WaituS(TYPICAL_PERIOD); @@ -274,7 +275,7 @@ int main() while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); break; case INSSETTINGS_ALGORITHM_ZEROGYROS: - zero_gyros(); + zero_gyros(false); // Run at standard rate while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); break; @@ -581,12 +582,21 @@ void measure_noise() } } -void zero_gyros() +void zero_gyros(bool update_settings) { const float rate = 1e-2; gyro_data.calibration.bias[0] += -gyro_data.filtered.x * rate; gyro_data.calibration.bias[1] += -gyro_data.filtered.y * 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); + } } /**