Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 00:58:37 -04:00
Code Issues Packages Projects Releases Wiki Activity

Speed IMU initialization

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1641 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
deweibel@gmail.com 2011-02-13 23:25:04 +00:00
parent 8e542512da
commit 0b6454aa0b
1 changed files with 11 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_IMU/AP_IMU_Oilpan.cpp
View File

@ -82,32 +82,32 @@ AP_IMU_Oilpan::init_gyro(Start_style style)
delay(500); delay(500);
Serial.println("Init Gyro"); Serial.println("Init Gyro");
for(int c = 0; c < 200; c++){ 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); digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH); digitalWrite(C_LED_PIN, HIGH);
delay(20); delay(20);
for (int i = 0; i < 6; i++) for (int i = 0; i < 6; i++)
adc_in[i] = _adc->Ch(_sensors[i]); adc_in[i] = _adc->Ch(_sensors[i]);
digitalWrite(A_LED_PIN, HIGH); digitalWrite(A_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, LOW); digitalWrite(C_LED_PIN, LOW);
delay(20); delay(20);
} }
for(int i = 0; i < 200; i++){ for (int j = 0; j <= 2; j++){
adc_in[j] -= _gyro_temp_comp(j, tc_temp);
_adc_offset[j] = adc_in[j];
}
for(int i = 0; i < 50; i++){
for (int j = 0; j <= 2; j++){ for (int j = 0; j <= 2; j++){
adc_in[j] = _adc->Ch(_sensors[j]); adc_in[j] = _adc->Ch(_sensors[j]);
// Subtract temp compensated typical gyro bias // Subtract temp compensated typical gyro bias
adc_in[j] -= _gyro_temp_comp(j, tc_temp); adc_in[j] -= _gyro_temp_comp(j, tc_temp);
// filter // filter
_adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1; _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1;
//Serial.print(_adc_offset[j], 1);
//Serial.print(", ");
} }
//Serial.println(" ");
delay(20); delay(20);
if(flashcount == 5) { if(flashcount == 5) {
@ -123,7 +123,6 @@ AP_IMU_Oilpan::init_gyro(Start_style style)
} }
flashcount++; flashcount++;
} }
Serial.println(" ");
_save_gyro_cal(); _save_gyro_cal();
} }
@ -151,28 +150,20 @@ AP_IMU_Oilpan::init_accel(Start_style style) // 3, 4, 5
for (int j = 3; j <= 5; j++){ for (int j = 3; j <= 5; j++){
adc_in[j] = _adc->Ch(_sensors[j]); adc_in[j] = _adc->Ch(_sensors[j]);
adc_in[j] -= 2025; // XXX bias value? adc_in[j] -= 2025; // Typical accel bias value - subtracted in _accel_in() and update()
_adc_offset[j] = adc_in[j]; _adc_offset[j] = adc_in[j];
} }
for(int i = 0; i < 200; i++){ // We take some readings... for(int i = 0; i < 50; i++){ // We take some readings...
delay(20); delay(20);
for (int j = 3; j <= 5; j++){ for (int j = 3; j <= 5; j++){
adc_in[j] = _adc->Ch(_sensors[j]); adc_in[j] = _adc->Ch(_sensors[j]);
adc_in[j] -= 2025; adc_in[j] -= 2025; // Typical accel bias value - subtracted in _accel_in() and update()
_adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1; _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1;
//Serial.print(j);
//Serial.print(": ");
//Serial.print(adc_in[j], 1);
//Serial.print(" | ");
//Serial.print(_adc_offset[j], 1);
//Serial.print(", ");
} }
//Serial.println(" ");
if(flashcount == 5) { if(flashcount == 5) {
Serial.print("*"); Serial.print("*");
digitalWrite(A_LED_PIN, LOW); digitalWrite(A_LED_PIN, LOW);