From 489365e7fc15df8db4e7846d94e65caa82665b68 Mon Sep 17 00:00:00 2001 From: deweibel Date: Sun, 24 Oct 2010 18:37:56 +0000 Subject: [PATCH] Add IMU library git-svn-id: https://arducopter.googlecode.com/svn/trunk@726 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_IMU/AP_IMU.cpp | 165 ++++++++++++++++++++++++++++++++++++ libraries/AP_IMU/AP_IMU.h | 48 +++++++++++ 2 files changed, 213 insertions(+) create mode 100644 libraries/AP_IMU/AP_IMU.cpp create mode 100644 libraries/AP_IMU/AP_IMU.h diff --git a/libraries/AP_IMU/AP_IMU.cpp b/libraries/AP_IMU/AP_IMU.cpp new file mode 100644 index 0000000000..84e9ce1f5b --- /dev/null +++ b/libraries/AP_IMU/AP_IMU.cpp @@ -0,0 +1,165 @@ + +#include "AP_IMU.h" + +#define A_LED_PIN 37 //37 = A, 35 = C +#define C_LED_PIN 35 + +// ADC : Voltage reference 3.3v / 12bits(4096 steps) => 0.8mV/ADC step +// ADXL335 Sensitivity(from datasheet) => 330mV/g, 0.8mV/ADC step => 330/0.8 = 412 +// Tested value : 418 +#define GRAVITY 418 //this equivalent to 1G in the raw data coming from the accelerometer +#define accel_scale(x) x*(9.81/GRAVITY)//Scaling the raw data of the accel to actual acceleration in meters per second squared + +#define ToRad(x) (x*0.01745329252) // *pi/180 +#define ToDeg(x) (x*57.2957795131) // *180/pi + +// IDG500 Sensitivity (from datasheet) => 2.0mV/ยบ/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 +// Tested values : 0.4026, ?, 0.4192 +#define _gyro_gain_x 0.4 //X axis Gyro gain +#define _gyro_gain_y 0.41 //Y axis Gyro gain +#define _gyro_gain_z 0.41 //Z axis Gyro + +#define ADC_CONSTRAINT 900 + +// Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ +const uint8_t AP_IMU::_sensors[6] = {1,2,0,4,5,6}; // For ArduPilot Mega Sensor Shield Hardware +const int AP_IMU::_sensor_signs[] = { 1, -1, -1, + 1, -1, -1}; + +// Temp compensation curve constants +// These must be produced by measuring data and curve fitting +// [X/Y/Z gyro][A/B/C or 0 order/1st order/2nd order constants] +const float AP_IMU::_gyro_temp_curve[3][3] = { + {1665,0,0}, + {1665,0,0}, + {1665,0,0} +}; // To Do - make additional constructors to pass this in. + + + +// Constructors //////////////////////////////////////////////////////////////// + +AP_IMU::AP_IMU(void) +{ +} + + +/**************************************************/ +void +AP_IMU::quick_init(void) +{ + +// NOTE *** Need to addd code to retrieve values from EEPROM +} + + +/**************************************************/ +void +AP_IMU::init(void) +{ + + float temp; + int flashcount = 0; + int tc_temp = APM_ADC.Ch(_gyro_temp_ch); + delay(500); + + for(int c = 0; c < 200; c++) + { + digitalWrite(A_LED_PIN, LOW); + digitalWrite(C_LED_PIN, HIGH); + delay(20); + for (int i = 0; i < 6; i++) + _adc_in[i] = APM_ADC.Ch(_sensors[i]); + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + delay(20); + } + + for(int y = 0; y <= 5; y++){ // Read first initial ADC values for offset. + _adc_offset[y] = _adc_in[y]; + } + + for(int i = 0; i < 200; i++){ // We take some readings... + for (int j = 0; j < 6; j++) { + _adc_in[j] = APM_ADC.Ch(_sensors[j]); + _adc_in[j] -= _gyro_temp_comp(j, tc_temp); // Subtract temp compensated typical gyro bias + if (_sensor_signs[j] < 0) + temp = (_adc_offset[j] - _adc_in[j]); + else + temp = (_adc_in[j] - _adc_offset[j]); + _adc_offset[j] = _adc_offset[j] * 0.9 + temp * 0.1; + } + + delay(20); + if(flashcount == 5) { + digitalWrite(A_LED_PIN, LOW); + digitalWrite(C_LED_PIN, HIGH); + } + if(flashcount >= 10) { + flashcount = 0; + digitalWrite(C_LED_PIN, LOW); + digitalWrite(A_LED_PIN, HIGH); + } + flashcount++; + } + + _adc_offset[5] += GRAVITY * _sensor_signs[5]; + +// NOTE *** Need to addd code to save values to EEPROM + +} + + +/**************************************************/ +Vector3f +AP_IMU::get_gyro(void) +{ + float temp; + int tc_temp = APM_ADC.Ch(_gyro_temp_ch); + + for (int i = 0; i < 3; i++) { + _adc_in[i] = APM_ADC.Ch(_sensors[i]); + _adc_in[i] -= _gyro_temp_comp(i, tc_temp); // Subtract temp compensated typical gyro bias + if (_sensor_signs[i] < 0) + temp = (_adc_offset[i] - _adc_in[i]); + else + temp = (_adc_in[i] - _adc_offset[i]); + if (abs(temp) > ADC_CONSTRAINT) { + adc_constraints++; // We keep track of the number of times + _adc_in[i] = constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values + } + } + + _gyro_vector.x = ToRad(_gyro_gain_x) * _adc_in[0]; + _gyro_vector.y = ToRad(_gyro_gain_y) * _adc_in[1]; + _gyro_vector.z = ToRad(_gyro_gain_z) * _adc_in[2]; + + return _gyro_vector; +} + +/**************************************************/ +Vector3f +AP_IMU::get_accel(void) +{ + float temp; + + for (int i = 3; i < 6; i++) { + _adc_in[i] = APM_ADC.Ch(_sensors[i]); + _adc_in[i] -= 2025; // Subtract typical accel bias + if (_sensor_signs[i] < 0) + temp = (_adc_offset[i] - _adc_in[i]); + else + temp = (_adc_in[i] - _adc_offset[i]); + if (abs(temp) > ADC_CONSTRAINT) { + adc_constraints++; // We keep track of the number of times + _adc_in[i] = constrain(temp, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values + } + } + + _accel_vector.x = accel_scale(_adc_in[3]); + _accel_vector.y = accel_scale(_adc_in[4]); + _accel_vector.z = accel_scale(_adc_in[5]); + + return _accel_vector; +} + diff --git a/libraries/AP_IMU/AP_IMU.h b/libraries/AP_IMU/AP_IMU.h new file mode 100644 index 0000000000..3da188f7c7 --- /dev/null +++ b/libraries/AP_IMU/AP_IMU.h @@ -0,0 +1,48 @@ +#ifndef AP_IMU_h +#define AP_IMU_h + +//#include +#include +#include +#include "WProgram.h" +#include + + +class AP_IMU +{ +public: + // Constructors + AP_IMU(); // Default Constructor + + // Methods + void quick_init(void); // For air start + void init(void); // For ground start + Vector3f get_gyro(void); // Radians/second + Vector3f get_accel(void); // G units + + // Members + uint8_t gyro_sat_count; + uint8_t adc_constraints; + +private: + // Methods + void read_adc_raw(void); + float _gyro_temp_comp(int i, int temp) const; + float read_adc(int select); + + // members + float _adc_in[6]; // array that store the 6 ADC channels used by IMU + float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers + Vector3f _accel_vector; // Store the acceleration in a vector + Vector3f _gyro_vector; //Store the gyros turn rate in a vector + + // constants + static const uint8_t _sensors[6]; + static const int _sensor_signs[9]; + static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature + static const float _gyro_temp_curve[3][3]; +}; + +#endif + +