From ec7c508207df43c8d8c49fb7ed076a775722090f Mon Sep 17 00:00:00 2001 From: "deweibel@gmail.com" Date: Fri, 25 Feb 2011 17:49:31 +0000 Subject: [PATCH] Change gyro/accel calibration to guard against improper orientations or motion during cal git-svn-id: https://arducopter.googlecode.com/svn/trunk@1727 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_IMU/AP_IMU_Oilpan.cpp | 141 +++++++++++++++++------------ libraries/AP_IMU/AP_IMU_Oilpan.h | 13 ++- 2 files changed, 95 insertions(+), 59 deletions(-) diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index 6149817823..8ecbcf602a 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -36,9 +36,9 @@ const int8_t AP_IMU_Oilpan::_sensor_signs[6] = { 1,-1,-1, 1,-1,-1}; // Channel // [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants] // const float AP_IMU_Oilpan::_gyro_temp_curve[3][3] = { - {1665,0,0}, - {1665,0,0}, - {1665,0,0} + {1658,0,0}, // Values to use if no temp compensation data available + {1658,0,0}, // Based on average values for 20 sample boards + {1658,0,0} }; void @@ -74,11 +74,14 @@ AP_IMU_Oilpan::_init_gyro() int flashcount = 0; int tc_temp; float adc_in; + float prev[3] = {0,0,0}; + float total_change; + float max_offset; // cold start tc_temp = _adc->Ch(_gyro_temp_ch); delay(500); - Serial.println("Init Gyro"); + Serial.printf_P(PSTR("Init Gyro")); for(int c = 0; c < 25; c++){ // Mostly we are just flashing the LED's here to tell the user to keep the IMU still digitalWrite(A_LED_PIN, LOW); @@ -93,34 +96,45 @@ AP_IMU_Oilpan::_init_gyro() delay(20); } - for (int j = 0; j <= 2; j++){ - adc_in -= _sensor_compensation(j, tc_temp); - _sensor_cal[j] = adc_in; - } - - for(int i = 0; i < 100; i++){ - for (int j = 0; j < 3; j++){ + for (int j=0; j<=2; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time + + do { + for (int j = 0; j <= 2; j++){ + prev[j] = _sensor_cal[j]; adc_in = _adc->Ch(_sensors[j]); - // Subtract temp compensated typical gyro bias adc_in -= _sensor_compensation(j, tc_temp); - // filter - _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; + _sensor_cal[j] = adc_in; } - delay(20); - if(flashcount == 5) { - Serial.print("*"); - digitalWrite(A_LED_PIN, LOW); - digitalWrite(C_LED_PIN, HIGH); - } + for(int i = 0; i < 50; i++){ + for (int j = 0; j < 3; j++){ + adc_in = _adc->Ch(_sensors[j]); + // Subtract temp compensated typical gyro bias + adc_in -= _sensor_compensation(j, tc_temp); + // filter + _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; + } - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(C_LED_PIN, LOW); + delay(20); + if(flashcount == 5) { + Serial.printf_P(PSTR("*")); + digitalWrite(A_LED_PIN, LOW); + digitalWrite(C_LED_PIN, HIGH); + } + + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(C_LED_PIN, LOW); digitalWrite(A_LED_PIN, HIGH); + } + flashcount++; } - flashcount++; - } + + total_change = fabs(prev[0] - _sensor_cal[0]) + fabs(prev[1] - _sensor_cal[1]) +fabs(prev[2] - _sensor_cal[2]); + max_offset = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1]; + max_offset = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2]; + delay(500); + } while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset); } void @@ -141,47 +155,60 @@ AP_IMU_Oilpan::_init_accel() { int flashcount = 0; float adc_in; + float prev[6] = {0,0,0}; + float total_change; + float max_offset; // cold start delay(500); - Serial.println("Init Accel"); + Serial.printf_P(PSTR("Init Accel")); - // init to initial reading (unlike gyro which presumes zero...) - // - for (int j = 3; j < 6; j++){ - adc_in = _adc->Ch(_sensors[j]); - adc_in -= _sensor_compensation(j, 0); // XXX secret knowledge, temperature ignored - _sensor_cal[j] = adc_in; - } - - for(int i = 0; i < 50; i++){ // We take some readings... - - delay(20); - - for (int j = 3; j < 6; j++){ - adc_in = _adc->Ch(_sensors[j]); - adc_in -= _sensor_compensation(j, 0); // XXX secret knowledge, temperature ignored - _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; + for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time + + do { + for (int j = 3; j <= 5; j++){ + prev[j] = _sensor_cal[j]; + adc_in = _adc->Ch(_sensors[j]); + adc_in -= _sensor_compensation(j, 0); // temperature ignored + _sensor_cal[j] = adc_in; } - if(flashcount == 5) { - Serial.print("*"); - digitalWrite(A_LED_PIN, LOW); - digitalWrite(C_LED_PIN, HIGH); + for(int i = 0; i < 50; i++){ // We take some readings... + + delay(20); + + for (int j = 3; j < 6; j++){ + adc_in = _adc->Ch(_sensors[j]); + adc_in -= _sensor_compensation(j, 0); // temperature ignored + _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; + } + + if(flashcount == 5) { + Serial.printf_P(PSTR("*")); + digitalWrite(A_LED_PIN, LOW); + digitalWrite(C_LED_PIN, HIGH); + } + + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + } + flashcount++; } - if(flashcount >= 10) { - flashcount = 0; - digitalWrite(C_LED_PIN, LOW); - digitalWrite(A_LED_PIN, HIGH); - } - flashcount++; - } - Serial.println(" "); + // null gravity from the Z accel + _sensor_cal[5] += _gravity * _sensor_signs[5]; + + total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]); + max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4]; + max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5]; - // null gravity from the Z accel - _sensor_cal[5] += _gravity * _sensor_signs[5]; + delay(500); + } while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset); + + Serial.printf_P(PSTR(" ")); } /**************************************************/ @@ -200,7 +227,7 @@ AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const } // do fixed-offset accelerometer compensation - return 2025; // XXX magic number! + return 2041; // Average raw value from a 20 board sample } float diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index 0909fdca18..d37bc69015 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -81,8 +81,9 @@ private: // ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 // Tested value : 418 // - static const float _gravity = 418.0; ///< 1G in the raw data coming from the accelerometer - static const float _accel_scale = 9.80665 / 418.0; ///< would like to use _gravity here, but cannot + static const float _gravity = 423.8; ///< 1G in the raw data coming from the accelerometer + // Value based on actual sample data from 20 boards + static const float _accel_scale = 9.80665 / 423.8; ///< would like to use _gravity here, but cannot // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 // Tested values : 0.4026, ?, 0.4192 @@ -94,6 +95,14 @@ private: // Maximum possible value returned by an offset-corrected sensor channel // static const float _adc_constraint = 900; + + // Gyro and Accelerometer calibration criterial + // + static const float _gyro_total_cal_change = 4.0; // Experimentally derived - allows for some minor motion + static const float _gyro_max_cal_offset = 320.0; + static const float _accel_total_cal_change = 4.0; + static const float _accel_max_cal_offset = 250.0; + }; #endif