|
|
|
@ -171,19 +171,19 @@ AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)
@@ -171,19 +171,19 @@ AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)
|
|
|
|
|
_ins->update(); |
|
|
|
|
_ins->get_accels(ins_accel); |
|
|
|
|
|
|
|
|
|
for (int j = 3; j <= 5; j++){ |
|
|
|
|
for (int j = 3; j <= 5; j++) { |
|
|
|
|
prev[j] = _sensor_cal[j]; |
|
|
|
|
adc_in = ins_accel[j-3]; |
|
|
|
|
_sensor_cal[j] = adc_in; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
for(int i = 0; i < 50; i++){ // We take some readings...
|
|
|
|
|
for(int i = 0; i < 50; i++) { // We take some readings...
|
|
|
|
|
|
|
|
|
|
delay_cb(20); |
|
|
|
|
_ins->update(); |
|
|
|
|
_ins->get_accels(ins_accel); |
|
|
|
|
|
|
|
|
|
for (int j = 3; j < 6; j++){ |
|
|
|
|
for (int j = 3; j < 6; j++) { |
|
|
|
|
adc_in = ins_accel[j-3]; |
|
|
|
|
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; |
|
|
|
|
} |
|
|
|
@ -213,7 +213,7 @@ AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)
@@ -213,7 +213,7 @@ AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)
|
|
|
|
|
Serial.printf_P(PSTR(" ")); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
float |
|
|
|
|
float |
|
|
|
|
AP_IMU_INS::_calibrated(uint8_t channel, float ins_value) |
|
|
|
|
{ |
|
|
|
|
return ins_value - _sensor_cal[channel]; |
|
|
|
|