diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp index e29fe5ca97..819662756c 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.cpp +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -1,8 +1,8 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // // // AP_IMU.cpp - IMU Sensor Library for Ardupilot Mega -// Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com +// Code by Michael Smith, Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com // // This library works with the ArduPilot Mega and "Oilpan" // @@ -22,60 +22,58 @@ #include "AP_IMU_Oilpan.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.0 // 1G in the raw data coming from the accelerometer -#define accel_scale(x) (x*9.80665/GRAVITY) // Scaling the raw data of the accel to actual acceleration in m/s/s - -// 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 +// XXX secret knowledge about the APM/oilpan wiring +// +#define A_LED_PIN 37 +#define C_LED_PIN 35 // Sensors: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ -const uint8_t AP_IMU_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6}; // For ArduPilot Mega Sensor Shield Hardware -const int8_t AP_IMU_Oilpan::_sensor_signs[6] = { 1,-1,-1, 1,-1,-1}; +const uint8_t AP_IMU_Oilpan::_sensors[6] = { 1, 2, 0, 4, 5, 6}; // Channel assignments on the APM oilpan +const int8_t AP_IMU_Oilpan::_sensor_signs[6] = { 1,-1,-1, 1,-1,-1}; // Channel orientation vs. normal // 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_Oilpan::_gyro_temp_curve[3][3] = { {1665,0,0}, {1665,0,0}, {1665,0,0} -}; // To Do - make additional constructors to pass this in. +}; void AP_IMU_Oilpan::init(Start_style style) { - init_gyro(style); - init_accel(style); + // if we are warm-starting, load the calibration data from EEPROM and go + // + if (WARM_START == style) { + _sensor_cal.load(); + } else { + + // do cold-start calibration for both accel and gyro + _init_gyro(); + _init_accel(); + + // save calibration + _sensor_cal.save(); + } } /**************************************************/ void -AP_IMU_Oilpan::init_gyro(Start_style style) +AP_IMU_Oilpan::init_gyro() +{ + _init_gyro(); + _sensor_cal.save(); +} + +void +AP_IMU_Oilpan::_init_gyro() { - float temp; int flashcount = 0; int tc_temp; - float adc_in[6]; - - // warm start, load saved cal from EEPROM - if ((WARM_START == style) && (0 != _address)) { - _adc_offset[0] = read_EE_float(_address ); - _adc_offset[1] = read_EE_float(_address + 4); - _adc_offset[2] = read_EE_float(_address + 8); - return; - } + float adc_in; // cold start tc_temp = _adc->Ch(_gyro_temp_ch); @@ -88,7 +86,7 @@ AP_IMU_Oilpan::init_gyro(Start_style style) delay(20); for (int i = 0; i < 6; i++) - adc_in[i] = _adc->Ch(_sensors[i]); + adc_in = _adc->Ch(_sensors[i]); digitalWrite(A_LED_PIN, HIGH); digitalWrite(C_LED_PIN, LOW); @@ -96,17 +94,17 @@ AP_IMU_Oilpan::init_gyro(Start_style style) } for (int j = 0; j <= 2; j++){ - adc_in[j] -= _gyro_temp_comp(j, tc_temp); - _adc_offset[j] = adc_in[j]; + adc_in -= _sensor_compensation(j, tc_temp); + _sensor_cal[j] = adc_in; } for(int i = 0; i < 50; i++){ - for (int j = 0; j <= 2; j++){ - adc_in[j] = _adc->Ch(_sensors[j]); + for (int j = 0; j < 3; j++){ + adc_in = _adc->Ch(_sensors[j]); // Subtract temp compensated typical gyro bias - adc_in[j] -= _gyro_temp_comp(j, tc_temp); + adc_in -= _sensor_compensation(j, tc_temp); // filter - _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1; + _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; } delay(20); @@ -123,45 +121,42 @@ AP_IMU_Oilpan::init_gyro(Start_style style) } flashcount++; } - - _save_gyro_cal(); } +void +AP_IMU_Oilpan::init_accel() +{ + _init_accel(); + _sensor_cal.save(); +} void -AP_IMU_Oilpan::init_accel(Start_style style) // 3, 4, 5 +AP_IMU_Oilpan::_init_accel() { - float temp; int flashcount = 0; - float adc_in[6]; - - // warm start, load our saved cal from EEPROM - if ((WARM_START == style) && (0 != _address)) { - _adc_offset[3] = read_EE_float(_address + 12); - _adc_offset[4] = read_EE_float(_address + 16); - _adc_offset[5] = read_EE_float(_address + 20); - return; - } + float adc_in; // cold start delay(500); Serial.println("Init Accel"); - for (int j = 3; j <= 5; j++){ - adc_in[j] = _adc->Ch(_sensors[j]); - adc_in[j] -= 2025; // Typical accel bias value - subtracted in _accel_in() and update() - _adc_offset[j] = adc_in[j]; + // init to initial reading (unlike gyro which presumes zero...) + // + for (int j = 3; j < 6; j++){ + adc_in = _adc->Ch(_sensors[j]); + adc_in -= _sensor_compensation(j, 0); // XXX secret knowledge, temperature ignored + _sensor_cal[j] = adc_in; } for(int i = 0; i < 50; i++){ // We take some readings... delay(20); - for (int j = 3; j <= 5; j++){ - adc_in[j] = _adc->Ch(_sensors[j]); - adc_in[j] -= 2025; // Typical accel bias value - subtracted in _accel_in() and update() - _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1; + for (int j = 3; j < 6; j++){ + adc_in = _adc->Ch(_sensors[j]); + adc_in -= _sensor_compensation(j, 0); // XXX secret knowledge, temperature ignored + _sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1; } if(flashcount == 5) { @@ -178,192 +173,74 @@ AP_IMU_Oilpan::init_accel(Start_style style) // 3, 4, 5 flashcount++; } Serial.println(" "); - _adc_offset[5] += GRAVITY * _sensor_signs[5]; - _save_accel_cal(); -} - -void -AP_IMU_Oilpan::zero_accel(void) // 3, 4, 5 -{ - _adc_offset[3] = 0; - _adc_offset[4] = 0; - _adc_offset[5] = 0; - _save_accel_cal(); -} - -void -AP_IMU_Oilpan::_save_gyro_cal(void) -{ - // save cal to EEPROM for warm start - if (0 != _address) { - write_EE_float(_adc_offset[0], _address); - write_EE_float(_adc_offset[1], _address + 4); - write_EE_float(_adc_offset[2], _address + 8); - } -} - -void -AP_IMU_Oilpan::_save_accel_cal(void) -{ - // save cal to EEPROM for warm start - if (0 != _address) { - write_EE_float(_adc_offset[3], _address + 12); - write_EE_float(_adc_offset[4], _address + 16); - write_EE_float(_adc_offset[5], _address + 20); - } + // null gravity from the Z accel + _sensor_cal[5] += _gravity * _sensor_signs[5]; } /**************************************************/ // Returns the temperature compensated raw gyro value //--------------------------------------------------- + float -AP_IMU_Oilpan::_gyro_temp_comp(int i, int temp) const +AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const { - // We use a 2nd order curve of the form Gtc = A + B * Graw + C * (Graw)**2 - //------------------------------------------------------------------------ - return _gyro_temp_curve[i][0] + _gyro_temp_curve[i][1] * temp + _gyro_temp_curve[i][2] * temp * temp; + // do gyro temperature compensation + if (channel < 3) { + + return _gyro_temp_curve[channel][0] + + _gyro_temp_curve[channel][1] * temperature + + _gyro_temp_curve[channel][2] * temperature * temperature; + } + + // do fixed-offset accelerometer compensation + return 2025; // XXX magic number! } float -AP_IMU_Oilpan::_gyro_in(uint8_t channel, int temperature) +AP_IMU_Oilpan::_sensor_in(uint8_t channel, int temperature) { - float adc_in; + float adc_in; - adc_in = _adc->Ch(_sensors[channel]); - adc_in -= _gyro_temp_comp(channel, temperature); // Subtract temp compensated typical gyro bias - if (_sensor_signs[channel] < 0) { - adc_in = _adc_offset[channel] - adc_in; - } else { - adc_in = adc_in - _adc_offset[channel]; - } + // get the compensated sensor value + // + adc_in = _adc->Ch(_sensors[channel]) - _sensor_compensation(channel, temperature); - if (fabs(adc_in) > ADC_CONSTRAINT) { - adc_constraints++; // We keep track of the number of times - adc_in = constrain(adc_in, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values - } - return adc_in; + // adjust for sensor sign and apply calibration offset + // + if (_sensor_signs[channel] < 0) { + adc_in = _sensor_cal[channel] - adc_in; + } else { + adc_in = adc_in - _sensor_cal[channel]; + } + + // constrain sensor readings to the sensible range + // + if (fabs(adc_in) > _adc_constraint) { + adc_constraints++; // We keep track of the number of times + adc_in = constrain(adc_in, -_adc_constraint, _adc_constraint); // Throw out nonsensical values + } + return adc_in; } -float -AP_IMU_Oilpan::_accel_in(uint8_t channel) -{ - float adc_in; - - adc_in = _adc->Ch(_sensors[channel]); - adc_in -= 2025; // Subtract typical accel bias - - if (_sensor_signs[channel] < 0) { - adc_in = _adc_offset[channel] - adc_in; - } else { - adc_in = adc_in - _adc_offset[channel]; - } - - if (fabs(adc_in) > ADC_CONSTRAINT) { - adc_constraints++; // We keep track of the number of times - adc_in = constrain(adc_in, -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values - } - return adc_in; -} bool AP_IMU_Oilpan::update(void) { int tc_temp = _adc->Ch(_gyro_temp_ch); - float adc_in[6]; -#if 0 - // get current gyro readings - for (int i = 0; i < 3; i++) { - adc_in[i] = _adc->Ch(_sensors[i]); - adc_in[i] -= _gyro_temp_comp(i,tc_temp); // Subtract temp compensated typical gyro bias - if (_sensor_signs[i] < 0) - adc_in[i] = (_adc_offset[i] - adc_in[i]); - else - adc_in[i] = (adc_in[i] - _adc_offset[i]); - if (fabs(adc_in[i]) > ADC_CONSTRAINT) { - adc_constraints++; // We keep track of the number of times - adc_in[i] = constrain(adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values - } - } -#endif - _gyro.x = ToRad(_gyro_gain_x) * _gyro_in(0, tc_temp); - _gyro.y = ToRad(_gyro_gain_y) * _gyro_in(1, tc_temp); - _gyro.z = ToRad(_gyro_gain_z) * _gyro_in(2, tc_temp); -#if 0 - // get current accelerometer readings - for (int i = 3; i < 6; i++) { - adc_in[i] = _adc->Ch(_sensors[i]); - adc_in[i] -= 2025; // Subtract typical accel bias + // convert corrected gyro readings to delta acceleration + // + _gyro.x = ToRad(_gyro_gain_x) * _sensor_in(0, tc_temp); + _gyro.y = ToRad(_gyro_gain_y) * _sensor_in(1, tc_temp); + _gyro.z = ToRad(_gyro_gain_z) * _sensor_in(2, tc_temp); - if (_sensor_signs[i] < 0) - adc_in[i] = _adc_offset[i] - adc_in[i]; - else - adc_in[i] = adc_in[i] - _adc_offset[i]; - - if (fabs(adc_in[i]) > ADC_CONSTRAINT) { - adc_constraints++; // We keep track of the number of times - adc_in[i] = constrain(adc_in[i], -ADC_CONSTRAINT, ADC_CONSTRAINT); // Throw out nonsensical values - } - } -#endif - _accel.x = accel_scale(_accel_in(3)); - _accel.y = accel_scale(_accel_in(4)); - _accel.z = accel_scale(_accel_in(5)); + // convert corrected accelerometer readings to acceleration + // + _accel.x = _accel_scale * _sensor_in(3, tc_temp); + _accel.y = _accel_scale * _sensor_in(4, tc_temp); + _accel.z = _accel_scale * _sensor_in(5, tc_temp); // always updated return true; } - -/********************************************************************************/ - -void -AP_IMU_Oilpan::print_accel_offsets(void) -{ - Serial.print("Accel offsets: "); - Serial.print(_adc_offset[3], 2); - Serial.print(", "); - Serial.print(_adc_offset[4], 2); - Serial.print(", "); - Serial.println(_adc_offset[5], 2); -} - -void -AP_IMU_Oilpan::print_gyro_offsets(void) -{ - Serial.print("Gyro offsets: "); - Serial.print(_adc_offset[0], 2); - Serial.print(", "); - Serial.print(_adc_offset[1], 2); - Serial.print(", "); - Serial.println(_adc_offset[2], 2); -} - -/********************************************************************************/ - -float -AP_IMU_Oilpan::read_EE_float(int address) -{ - union { - byte bytes[4]; - float value; - } _floatOut; - - for (int i = 0; i < 4; i++) - _floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i)); - return _floatOut.value; -} - -void -AP_IMU_Oilpan::write_EE_float(float value, int address) -{ - union { - byte bytes[4]; - float value; - } _floatIn; - - _floatIn.value = value; - for (int i = 0; i < 4; i++) - eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]); -} - diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.h b/libraries/AP_IMU/AP_IMU_Oilpan.h index 707b9d6efc..6865022889 100644 --- a/libraries/AP_IMU/AP_IMU_Oilpan.h +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -1,4 +1,4 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// @file AP_IMU_Oilpan.h /// @brief IMU driver for the APM oilpan @@ -6,61 +6,79 @@ #ifndef AP_IMU_Oilpan_h #define AP_IMU_Oilpan_h -#include "IMU.h" +#include #include #include #include +#include "IMU.h" + class AP_IMU_Oilpan : public IMU { public: - AP_IMU_Oilpan(AP_ADC *adc, uint16_t address) : - _adc(adc), - _address(address) + /// Constructor + /// + /// Saves the ADC pointer and constructs the calibration data variable. + /// + /// @param adc Pointer to the AP_ADC instance that is connected to the gyro and accelerometer. + /// @param key The AP_Var::key value we will use when loading/saving calibration data. + /// + AP_IMU_Oilpan(AP_ADC *adc, AP_Var::Key key) : + _adc(adc), + _sensor_cal(key, PSTR("IMU_SENSOR_CAL"), AP_Var::k_flag_no_auto_load) {} + /// Do warm or cold start. + /// + /// @note For a partial-warmstart where e.g. the accelerometer calibration should be preserved + /// but the gyro cal needs to be re-performed, start with ::init(WARM_START) to load the + /// previous calibration settings, then force a re-calibration of the gyro with ::init_gyro. + /// + /// @param style Selects the initialisation style. + /// COLD_START performs calibration of both the accelerometer and gyro. + /// WARM_START loads accelerometer and gyro calibration from a previous cold start. + /// virtual void init(Start_style style = COLD_START); - virtual void init_accel(Start_style style = COLD_START); - virtual void init_gyro(Start_style style = COLD_START); + + virtual void init_accel(); + virtual void init_gyro(); virtual bool update(void); - // XXX backwards compat hacks - void zero_accel(void); - - void print_accel_offsets(void); ///< XXX debug hack - void print_gyro_offsets(void); ///< XXX debug hack - - int ax() { return _adc_offset[3]; } - int ay() { return _adc_offset[4]; } - int az() { return _adc_offset[5]; } - - void ax(const int v) { _adc_offset[3] = v; } - void ay(const int v) { _adc_offset[4] = v; } - void az(const int v) { _adc_offset[5] = v; } - private: - float _gyro_temp_comp(int i, int temp) const; - void _save_gyro_cal(void); - void _save_accel_cal(void); + AP_ADC *_adc; ///< ADC that we use for reading sensors + AP_VarA _sensor_cal; ///< Calibrated sensor offsets - float _gyro_in(uint8_t channel, int temperature); - float _accel_in(uint8_t channel); + virtual void _init_accel(); ///< no-save implementation + virtual void _init_gyro(); ///< no-save implementation - AP_ADC *_adc; // Analog to digital converter pointer - uint16_t _address; // EEPROM start address for saving/retrieving offsets - float _adc_offset[6]; // Array that store the Offset of the gyros and accelerometers - - // XXX should not be implementing these here - float read_EE_float(int address); - void write_EE_float(float value, int address); + float _sensor_compensation(uint8_t channel, int temp) const; + float _sensor_in(uint8_t channel, int temperature); // constants - static const uint8_t _sensors[6]; - static const int8_t _sensor_signs[6]; - static const uint8_t _gyro_temp_ch = 3; // The ADC channel reading the gyro temperature - static const float _gyro_temp_curve[3][3]; + static const uint8_t _sensors[6]; ///< ADC channel mappings for the sensors + static const int8_t _sensor_signs[6]; ///< ADC result sign adjustment for sensors + static const uint8_t _gyro_temp_ch = 3; ///< ADC channel reading the gyro temperature + static const float _gyro_temp_curve[3][3]; ///< Temperature compensation curve for the gyro + + // 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 + // + static const float _gravity = 418.0; ///< 1G in the raw data coming from the accelerometer + static const float _accel_scale = 9.80665 / 418.0; ///< would like to use _gravity here, but cannot + + // IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s, 0.8mV/ADC step => 0.8/3.33 = 0.4 + // Tested values : 0.4026, ?, 0.4192 + // + static const float _gyro_gain_x = 0.4; // X axis Gyro gain + static const float _gyro_gain_y = 0.41; // Y axis Gyro gain + static const float _gyro_gain_z = 0.41; // Z axis Gyro gain + + // Maximum possible value returned by an offset-corrected sensor channel + // + static const float _adc_constraint = 900; }; #endif diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h index 1ae7266a9a..11e7d748a6 100644 --- a/libraries/AP_IMU/IMU.h +++ b/libraries/AP_IMU/IMU.h @@ -16,7 +16,7 @@ class IMU public: /// Constructor IMU() {} - + enum Start_style { COLD_START = 0, WARM_START @@ -24,7 +24,7 @@ public: /// Perform startup initialisation. /// - /// Called to initialise the state of the IMU. + /// Called to initialise the state of the IMU. /// /// For COLD_START, implementations using real sensors can assume /// that the airframe is stationary and nominally oriented. @@ -36,24 +36,20 @@ public: /// @param style The initialisation startup style. /// virtual void init(Start_style style) = 0; - - /// Perform startup initialisation for just the accelerometers. + + /// Perform cold startup initialisation for just the accelerometers. /// /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work. /// - /// @param style The initialisation startup style. - /// - virtual void init_accel(Start_style style) = 0; + virtual void init_accel() = 0; - /// Perform cold-start initialisation for just the gyros. + /// Perform cold-start initialisation for just the gyros. /// /// @note This should not be called unless ::init has previously /// been called, as ::init may perform other work /// - /// @param style The initialisation startup style. - /// - virtual void init_gyro(Start_style style) = 0; + virtual void init_gyro() = 0; /// Give the IMU some cycles to perform/fetch an update from its /// sensors. @@ -65,7 +61,7 @@ public: /// Fetch the current gyro values /// /// @returns vector of rotational rates in radians/sec - /// + /// Vector3f get_gyro(void) { return _gyro; } /// Fetch the current accelerometer values @@ -81,10 +77,6 @@ public: /// uint8_t adc_constraints; - // XXX backwards compat hacks - void load_gyro_eeprom(void) { init_accel(WARM_START); } ///< XXX backwards compat hack - void load_accel_eeprom(void) { init_gyro(WARM_START); } ///< XXX backwards compat hack - protected: /// Most recent accelerometer reading obtained by ::update Vector3f _accel;