00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00017
00018 #include <FastSerial.h>
00019 #include <AP_Common.h>
00020
00021 #include <avr/eeprom.h>
00022
00023 #include "AP_IMU_Oilpan.h"
00024
00025 #define A_LED_PIN 37 //37 = A, 35 = C
00026 #define C_LED_PIN 35
00027
00028
00029
00030
00031 #define GRAVITY 418.0 // 1G in the raw data coming from the accelerometer
00032 #define accel_scale(x) (x*9.80665/GRAVITY) // Scaling the raw data of the accel to actual acceleration in m/s/s
00033
00034
00035
00036 #define _gyro_gain_x 0.4 //X axis Gyro gain
00037 #define _gyro_gain_y 0.41 //Y axis Gyro gain
00038 #define _gyro_gain_z 0.41 //Z axis Gyro
00039
00040 #define ADC_CONSTRAINT 900
00041
00042
00043 const uint8_t AP_IMU_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6};
00044 const int8_t AP_IMU_Oilpan::_sensor_signs[6] = { 1,-1,-1, 1,-1,-1};
00045
00046
00047
00048
00049 const float AP_IMU_Oilpan::_gyro_temp_curve[3][3] = {
00050 {1665,0,0},
00051 {1665,0,0},
00052 {1665,0,0}
00053 };
00054
00055 void
00056 AP_IMU_Oilpan::init(Start_style style)
00057 {
00058 init_gyro(style);
00059 init_accel(style);
00060 }
00061
00062
00063
00064 void
00065 AP_IMU_Oilpan::init_gyro(Start_style style)
00066 {
00067 float temp;
00068 int flashcount = 0;
00069 int tc_temp;
00070 float adc_in[6];
00071
00072
00073 if ((WARM_START == style) && (0 != _address)) {
00074 _adc_offset[0] = read_EE_float(_address );
00075 _adc_offset[1] = read_EE_float(_address + 4);
00076 _adc_offset[2] = read_EE_float(_address + 8);
00077 return;
00078 }
00079
00080
00081 tc_temp = _adc->Ch(_gyro_temp_ch);
00082 delay(500);
00083 Serial.println("Init Gyro");
00084
00085 for(int c = 0; c < 200; c++){
00086 digitalWrite(A_LED_PIN, LOW);
00087 digitalWrite(C_LED_PIN, HIGH);
00088 delay(20);
00089
00090 for (int i = 0; i < 6; i++)
00091 adc_in[i] = _adc->Ch(_sensors[i]);
00092
00093 digitalWrite(A_LED_PIN, HIGH);
00094 digitalWrite(C_LED_PIN, LOW);
00095 delay(20);
00096 }
00097
00098 for(int i = 0; i < 200; i++){
00099 for (int j = 0; j <= 2; j++){
00100 adc_in[j] = _adc->Ch(_sensors[j]);
00101
00102
00103 adc_in[j] -= _gyro_temp_comp(j, tc_temp);
00104
00105
00106 _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1;
00107
00108
00109 }
00110
00111
00112 delay(20);
00113 if(flashcount == 5) {
00114 Serial.print("*");
00115 digitalWrite(A_LED_PIN, LOW);
00116 digitalWrite(C_LED_PIN, HIGH);
00117 }
00118
00119 if(flashcount >= 10) {
00120 flashcount = 0;
00121 digitalWrite(C_LED_PIN, LOW);
00122 digitalWrite(A_LED_PIN, HIGH);
00123 }
00124 flashcount++;
00125 }
00126 Serial.println(" ");
00127
00128 _save_gyro_cal();
00129 }
00130
00131
00132 void
00133 AP_IMU_Oilpan::init_accel(Start_style style)
00134 {
00135 float temp;
00136 int flashcount = 0;
00137 float adc_in[6];
00138
00139
00140 if ((WARM_START == style) && (0 != _address)) {
00141 _adc_offset[3] = read_EE_float(_address + 12);
00142 _adc_offset[4] = read_EE_float(_address + 16);
00143 _adc_offset[5] = read_EE_float(_address + 20);
00144 return;
00145 }
00146
00147
00148 delay(500);
00149
00150 Serial.println("Init Accel");
00151
00152 for (int j = 3; j <= 5; j++){
00153 adc_in[j] = _adc->Ch(_sensors[j]);
00154 adc_in[j] -= 2025;
00155 _adc_offset[j] = adc_in[j];
00156 }
00157
00158 for(int i = 0; i < 200; i++){
00159
00160 delay(20);
00161
00162 for (int j = 3; j <= 5; j++){
00163 adc_in[j] = _adc->Ch(_sensors[j]);
00164 adc_in[j] -= 2025;
00165 _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1;
00166
00167
00168
00169
00170
00171
00172 }
00173
00174
00175
00176 if(flashcount == 5) {
00177 Serial.print("*");
00178 digitalWrite(A_LED_PIN, LOW);
00179 digitalWrite(C_LED_PIN, HIGH);
00180 }
00181
00182 if(flashcount >= 10) {
00183 flashcount = 0;
00184 digitalWrite(C_LED_PIN, LOW);
00185 digitalWrite(A_LED_PIN, HIGH);
00186 }
00187 flashcount++;
00188 }
00189 Serial.println(" ");
00190 _adc_offset[5] += GRAVITY * _sensor_signs[5];
00191
00192 _save_accel_cal();
00193 }
00194
00195 void
00196 AP_IMU_Oilpan::zero_accel(void)
00197 {
00198 _adc_offset[3] = 0;
00199 _adc_offset[4] = 0;
00200 _adc_offset[5] = 0;
00201 _save_accel_cal();
00202 }
00203
00204 void
00205 AP_IMU_Oilpan::_save_gyro_cal(void)
00206 {
00207
00208 if (0 != _address) {
00209 write_EE_float(_adc_offset[0], _address);
00210 write_EE_float(_adc_offset[1], _address + 4);
00211 write_EE_float(_adc_offset[2], _address + 8);
00212 }
00213 }
00214
00215 void
00216 AP_IMU_Oilpan::_save_accel_cal(void)
00217 {
00218
00219 if (0 != _address) {
00220 write_EE_float(_adc_offset[3], _address + 12);
00221 write_EE_float(_adc_offset[4], _address + 16);
00222 write_EE_float(_adc_offset[5], _address + 20);
00223 }
00224 }
00225
00226
00227
00228
00229 float
00230 AP_IMU_Oilpan::_gyro_temp_comp(int i, int temp) const
00231 {
00232
00233
00234 return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp;
00235 }
00236
00237 float
00238 AP_IMU_Oilpan::_gyro_in(uint8_t channel, int temperature)
00239 {
00240 float adc_in;
00241
00242 adc_in = _adc->Ch(_sensors[channel]);
00243 adc_in -= _gyro_temp_comp(channel, temperature);
00244 if (_sensor_signs[channel] < 0) {
00245 adc_in = _adc_offset[channel] - adc_in;
00246 } else {
00247 adc_in = adc_in - _adc_offset[channel];
00248 }
00249
00250 if (fabs(adc_in) > ADC_CONSTRAINT) {
00251 adc_constraints++;
00252 adc_in = constrain(adc_in, -ADC_CONSTRAINT, ADC_CONSTRAINT);
00253 }
00254 return adc_in;
00255 }
00256
00257 float
00258 AP_IMU_Oilpan::_accel_in(uint8_t channel)
00259 {
00260 float adc_in;
00261
00262 adc_in = _adc->Ch(_sensors[channel]);
00263 adc_in -= 2025;
00264
00265 if (_sensor_signs[channel] < 0) {
00266 adc_in = _adc_offset[channel] - adc_in;
00267 } else {
00268 adc_in = adc_in - _adc_offset[channel];
00269 }
00270
00271 if (fabs(adc_in) > ADC_CONSTRAINT) {
00272 adc_constraints++;
00273 adc_in = constrain(adc_in, -ADC_CONSTRAINT, ADC_CONSTRAINT);
00274 }
00275 return adc_in;
00276 }
00277
00278 bool
00279 AP_IMU_Oilpan::update(void)
00280 {
00281 int tc_temp = _adc->Ch(_gyro_temp_ch);
00282 float adc_in[6];
00283 #if 0
00284
00285 for (int i = 0; i < 3; i++) {
00286 adc_in[i] = _adc->Ch(_sensors[i]);
00287 adc_in[i] -= _gyro_temp_comp(i,tc_temp);
00288 if (_sensor_signs[i] < 0)
00289 adc_in[i] = (_adc_offset[i] - adc_in[i]);
00290 else
00291 adc_in[i] = (adc_in[i] - _adc_offset[i]);
00292
00293 if (fabs(adc_in[i]) > ADC_CONSTRAINT) {
00294 adc_constraints++;
00295 adc_in[i] = constrain(adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);
00296 }
00297 }
00298 #endif
00299 _gyro.x = ToRad(_gyro_gain_x) * _gyro_in(0, tc_temp);
00300 _gyro.y = ToRad(_gyro_gain_y) * _gyro_in(1, tc_temp);
00301 _gyro.z = ToRad(_gyro_gain_z) * _gyro_in(2, tc_temp);
00302 #if 0
00303
00304 for (int i = 3; i < 6; i++) {
00305 adc_in[i] = _adc->Ch(_sensors[i]);
00306 adc_in[i] -= 2025;
00307
00308 if (_sensor_signs[i] < 0)
00309 adc_in[i] = _adc_offset[i] - adc_in[i];
00310 else
00311 adc_in[i] = adc_in[i] - _adc_offset[i];
00312
00313 if (fabs(adc_in[i]) > ADC_CONSTRAINT) {
00314 adc_constraints++;
00315 adc_in[i] = constrain(adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT);
00316 }
00317 }
00318 #endif
00319 _accel.x = accel_scale(_accel_in(3));
00320 _accel.y = accel_scale(_accel_in(4));
00321 _accel.z = accel_scale(_accel_in(5));
00322
00323
00324 return true;
00325 }
00326
00327
00328
00329 void
00330 AP_IMU_Oilpan::print_accel_offsets(void)
00331 {
00332 Serial.print("Accel offsets: ");
00333 Serial.print(_adc_offset[3], 2);
00334 Serial.print(", ");
00335 Serial.print(_adc_offset[4], 2);
00336 Serial.print(", ");
00337 Serial.println(_adc_offset[5], 2);
00338 }
00339
00340 void
00341 AP_IMU_Oilpan::print_gyro_offsets(void)
00342 {
00343 Serial.print("Gyro offsets: ");
00344 Serial.print(_adc_offset[0], 2);
00345 Serial.print(", ");
00346 Serial.print(_adc_offset[1], 2);
00347 Serial.print(", ");
00348 Serial.println(_adc_offset[2], 2);
00349 }
00350
00351
00352
00353 float
00354 AP_IMU_Oilpan::read_EE_float(int address)
00355 {
00356 union {
00357 byte bytes[4];
00358 float value;
00359 } _floatOut;
00360
00361 for (int i = 0; i < 4; i++)
00362 _floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
00363 return _floatOut.value;
00364 }
00365
00366 void
00367 AP_IMU_Oilpan::write_EE_float(float value, int address)
00368 {
00369 union {
00370 byte bytes[4];
00371 float value;
00372 } _floatIn;
00373
00374 _floatIn.value = value;
00375 for (int i = 0; i < 4; i++)
00376 eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
00377 }
00378