Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

just formatting 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1887 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-04-16 04:55:32 +00:00
parent e0834ed0f2
commit 12a78ecbdb
1 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_IMU/AP_IMU_Oilpan.cpp
View File

@ -96,13 +96,14 @@ AP_IMU_Oilpan::_init_gyro()
delay(20); delay(20);
} }
for (int j=0; j<=2; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time for (int j = 0; j <= 2; j++)
_sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do { do {
for (int j = 0; j <= 2; j++){ for (int j = 0; j <= 2; j++){
prev[j] = _sensor_cal[j]; prev[j] = _sensor_cal[j];
adc_in = _adc->Ch(_sensors[j]); adc_in = _adc->Ch(_sensors[j]);
adc_in -= _sensor_compensation(j, tc_temp); adc_in -= _sensor_compensation(j, tc_temp);
_sensor_cal[j] = adc_in; _sensor_cal[j] = adc_in;
} }
@ -125,14 +126,14 @@ AP_IMU_Oilpan::_init_gyro()
if(flashcount >= 10) { if(flashcount >= 10) {
flashcount = 0; flashcount = 0;
digitalWrite(C_LED_PIN, LOW); digitalWrite(C_LED_PIN, LOW);
digitalWrite(A_LED_PIN, HIGH); 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]); 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 = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1];
max_offset = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2]; max_offset = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2];
delay(500); delay(500);
} while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset); } while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
} }
@ -165,7 +166,7 @@ AP_IMU_Oilpan::_init_accel()
Serial.printf_P(PSTR("Init Accel")); Serial.printf_P(PSTR("Init Accel"));
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do { do {
for (int j = 3; j <= 5; j++){ for (int j = 3; j <= 5; j++){
prev[j] = _sensor_cal[j]; prev[j] = _sensor_cal[j];
@ -200,14 +201,14 @@ AP_IMU_Oilpan::_init_accel()
// null gravity from the Z accel // null gravity from the Z accel
_sensor_cal[5] += _gravity * _sensor_signs[5]; _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]); 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 = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5]; max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
delay(500); delay(500);
} while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset); } while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
Serial.printf_P(PSTR(" ")); Serial.printf_P(PSTR(" "));
} }