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