diff --git a/libraries/AP_IMU/AP_IMU.cpp b/libraries/AP_IMU/AP_IMU.cpp deleted file mode 100644 index abf078887b..0000000000 --- a/libraries/AP_IMU/AP_IMU.cpp +++ /dev/null @@ -1,333 +0,0 @@ -/* - AP_IMU.cpp - IMU Sensor Library for Ardupilot Mega - Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com - - This library works with the ArduPilot Mega and "Oilpan" - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Methods: - quick_init() : For air restart - init() : Calibration - gyro_init() : For ground start using saved accel offsets - get_gyro() : Returns gyro vector. Elements in radians/second - get_accel() : Returns acceleration vector. Elements in meters/seconds squared - -*/ - -#include - -#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.80665/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. - -void -AP_IMU::init(void) -{ - init_gyro(); - init_accel(); -} - -/**************************************************/ - -void -AP_IMU::init_gyro(void) -{ - - float temp; - int flashcount = 0; - int tc_temp = _adc->Ch(_gyro_temp_ch); - delay(500); - Serial.println("Init Gyro"); - - 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] = _adc->Ch(_sensors[i]); - - digitalWrite(A_LED_PIN, HIGH); - digitalWrite(C_LED_PIN, LOW); - delay(20); - } - - for(int i = 0; i < 200; i++){ - for (int j = 0; j <= 2; j++){ - _adc_in[j] = _adc->Ch(_sensors[j]); - - // Subtract temp compensated typical gyro bias - _adc_in[j] -= gyro_temp_comp(j, tc_temp); - - // filter - _adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1; - //Serial.print(_adc_offset[j], 1); - //Serial.print(", "); - } - //Serial.println(" "); - - delay(20); - if(flashcount == 5) { - Serial.print("*"); - 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++; - } - Serial.println(" "); - - save_gyro_eeprom(); -} - - -void -AP_IMU::init_accel(void) // 3, 4, 5 -{ - float temp; - int flashcount = 0; - delay(500); - - Serial.println("Init Accel"); - - for (int j = 3; j <= 5; j++){ - _adc_in[j] = _adc->Ch(_sensors[j]); - _adc_in[j] -= 2025; - _adc_offset[j] = _adc_in[j]; - } - - for(int i = 0; i < 200; 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; - _adc_offset[j] = _adc_offset[j] * 0.9 + _adc_in[j] * 0.1; - //Serial.print(j); - //Serial.print(": "); - //Serial.print(_adc_in[j], 1); - //Serial.print(" | "); - //Serial.print(_adc_offset[j], 1); - //Serial.print(", "); - } - - //Serial.println(" "); - - if(flashcount == 5) { - Serial.print("*"); - 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++; - } - Serial.println(" "); - _adc_offset[5] += GRAVITY * _sensor_signs[5]; - save_accel_eeprom(); -} - -void -AP_IMU::zero_accel(void) // 3, 4, 5 -{ - _adc_offset[3] = 0; - _adc_offset[4] = 0; - _adc_offset[5] = 0; - save_accel_eeprom(); -} -/**************************************************/ -// Returns the temperature compensated raw gyro value -//--------------------------------------------------- -float -AP_IMU::gyro_temp_comp(int i, int temp) 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; -} - -/**************************************************/ -Vector3f -AP_IMU::get_gyro(void) -{ - int tc_temp = _adc->Ch(_gyro_temp_ch); - - 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 - } - } - - _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) -{ - for (int i = 3; i < 6; i++) { - _adc_in[i] = _adc->Ch(_sensors[i]); - _adc_in[i] -= 2025; // Subtract typical accel 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 - } - } - - _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; -} - -/********************************************************************************/ - -void -AP_IMU::load_gyro_eeprom(void) -{ - _adc_offset[0] = read_EE_float(_address ); - _adc_offset[1] = read_EE_float(_address + 4); - _adc_offset[2] = read_EE_float(_address + 8); -} - -void -AP_IMU::save_gyro_eeprom(void) -{ - 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::load_accel_eeprom(void) -{ - _adc_offset[3] = read_EE_float(_address + 12); - _adc_offset[4] = read_EE_float(_address + 16); - _adc_offset[5] = read_EE_float(_address + 20); -} - -void -AP_IMU::save_accel_eeprom(void) -{ - write_EE_float(_adc_offset[3], _address + 12); - write_EE_float(_adc_offset[4], _address + 16); - write_EE_float(_adc_offset[5], _address + 20); -} - -void -AP_IMU::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::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::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::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.h b/libraries/AP_IMU/AP_IMU.h index a17e05ed02..d72ac2b631 100644 --- a/libraries/AP_IMU/AP_IMU.h +++ b/libraries/AP_IMU/AP_IMU.h @@ -1,70 +1,7 @@ -#ifndef AP_IMU_h -#define AP_IMU_h +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#include -#include -#include -#include "WProgram.h" -#include -#include +/// @file AP_IMU.h +/// @brief Catch-all header that defines all supported IMU classes. - -class AP_IMU -{ - -public: - // Constructors - AP_IMU(AP_ADC *adc, uint16_t address) : - _adc(adc), - _address(address) - {} - - // Methods - void init(void); // inits both - void init_accel(void); // just Accels - void init_gyro(void); // just gyros - void zero_accel(void); - - void load_gyro_eeprom(void); - void save_gyro_eeprom(void); - void load_accel_eeprom(void); - void save_accel_eeprom(void); - void print_accel_offsets(void); - void print_gyro_offsets(void); - - 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; } - - - // raw ADC values - called by DCM - Vector3f get_gyro(void); // Radians/second - Vector3f get_accel(void); // meters/seconds squared - - // Members - uint8_t adc_constraints; // a check of how many times we get non-sensical values - -private: - // Methods - void read_offsets(void); - float gyro_temp_comp(int i, int temp) const; - - // members - uint16_t _address; // EEPROM start address for saving/retrieving offsets - 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 - AP_ADC * _adc; // Analog to digital converter pointer - - float read_EE_float(int address); - void write_EE_float(float value, int address); - - // 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 +#include "AP_IMU_Oilpan.h" +#include "AP_IMU_Shim.h" diff --git a/libraries/AP_IMU/AP_IMU_Oilpan.cpp b/libraries/AP_IMU/AP_IMU_Oilpan.cpp new file mode 100644 index 0000000000..44617e6dcf --- /dev/null +++ b/libraries/AP_IMU/AP_IMU_Oilpan.cpp @@ -0,0 +1,378 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// +// +// AP_IMU.cpp - IMU Sensor Library for Ardupilot Mega +// Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com +// +// This library works with the ArduPilot Mega and "Oilpan" +// +// This library is free software; you can redistribute it and/or +// modify it under the terms of the GNU Lesser General Public +// License as published by the Free Software Foundation; either +// version 2.1 of the License, or (at your option) any later version. +// + +/// @file AP_IMU.h +/// @brief IMU driver for the APM oilpan + +#include +#include + +#include + +#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 + +// 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}; + +// 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); +} + +/**************************************************/ + +void +AP_IMU_Oilpan::init_gyro(Start_style style) +{ + 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; + } + + // cold start + tc_temp = _adc->Ch(_gyro_temp_ch); + delay(500); + Serial.println("Init Gyro"); + + 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] = _adc->Ch(_sensors[i]); + + digitalWrite(A_LED_PIN, HIGH); + digitalWrite(C_LED_PIN, LOW); + delay(20); + } + + for(int i = 0; i < 200; i++){ + for (int j = 0; j <= 2; j++){ + adc_in[j] = _adc->Ch(_sensors[j]); + + // Subtract temp compensated typical gyro bias + adc_in[j] -= _gyro_temp_comp(j, tc_temp); + + // filter + _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1; + //Serial.print(_adc_offset[j], 1); + //Serial.print(", "); + } + //Serial.println(" "); + + delay(20); + if(flashcount == 5) { + Serial.print("*"); + 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++; + } + Serial.println(" "); + + _save_gyro_cal(); +} + + +void +AP_IMU_Oilpan::init_accel(Start_style style) // 3, 4, 5 +{ + 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; + } + + // 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; // XXX bias value? + _adc_offset[j] = adc_in[j]; + } + + for(int i = 0; i < 200; 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; + _adc_offset[j] = _adc_offset[j] * 0.9 + adc_in[j] * 0.1; + //Serial.print(j); + //Serial.print(": "); + //Serial.print(adc_in[j], 1); + //Serial.print(" | "); + //Serial.print(_adc_offset[j], 1); + //Serial.print(", "); + } + + //Serial.println(" "); + + if(flashcount == 5) { + Serial.print("*"); + 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++; + } + 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); + } +} + +/**************************************************/ +// Returns the temperature compensated raw gyro value +//--------------------------------------------------- +float +AP_IMU_Oilpan::_gyro_temp_comp(int i, int temp) 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; +} + +float +AP_IMU_Oilpan::_gyro_in(uint8_t channel, int temperature) +{ + 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]; + } + + 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 + + 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)); + + // 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 new file mode 100644 index 0000000000..d13268f2ef --- /dev/null +++ b/libraries/AP_IMU/AP_IMU_Oilpan.h @@ -0,0 +1,62 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_IMU_Oilpan.h +/// @brief IMU driver for the APM oilpan + +#ifndef AP_IMU_Oilpan_h +#define AP_IMU_Oilpan_h + +#include "IMU.h" + +#include +#include +#include + +class AP_IMU_Oilpan : public IMU +{ + +public: + AP_IMU_Oilpan(AP_ADC *adc, uint16_t address) : + _adc(adc), + _address(address) + {} + + 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 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 + + 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); + + float _gyro_in(uint8_t channel, int temperature); + float _accel_in(uint8_t channel); + + 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); + + // 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]; +}; + +#endif diff --git a/libraries/AP_IMU/AP_IMU_Shim.h b/libraries/AP_IMU/AP_IMU_Shim.h new file mode 100644 index 0000000000..be02e27565 --- /dev/null +++ b/libraries/AP_IMU/AP_IMU_Shim.h @@ -0,0 +1,12 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_IMU_Shim.h +/// @brief IMU shim driver, used when the IMU data is coming from somewhere else. + +#ifndef AP_IMU_Shim_h #define AP_IMU_Shim_h class AP_IMU_Shim : public IMU { public: AP_IMU_Shim(void) {} /// @name IMU protocol //@{ virtual void init(Start_style style) {} + virtual void init_accel(Start_style style) {}; + virtual void init_gyro(Start_style style) {}; + virtual bool update(void) { bool updated = _updated; + _updated = false; return updated; } //@} /// Set the gyro vector. ::update will return /// true once after this call. /// /// @param v The new gyro vector. /// void set_gyro(Vector3f v) { _gyro = v; _updated = true; } /// Set the accelerometer vector. ::update will return /// true once after this call. /// /// @param v The new accelerometer vector. /// void set_accel(Vector3f v) { _accel = v; _updated = true; } private: /// set true when new data is delivered bool _updated; }; + +#endif diff --git a/libraries/AP_IMU/IMU.h b/libraries/AP_IMU/IMU.h new file mode 100644 index 0000000000..1ae7266a9a --- /dev/null +++ b/libraries/AP_IMU/IMU.h @@ -0,0 +1,96 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file IMU.h +/// @brief Abstract class defining the interface to a real or virtual +/// Inertial Measurement Unit. + +#ifndef IMU_h +#define IMU_h + +#include +#include + +class IMU +{ + +public: + /// Constructor + IMU() {} + + enum Start_style { + COLD_START = 0, + WARM_START + }; + + /// Perform startup initialisation. + /// + /// 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. + /// + /// For WARM_START, no assumptions should be made about the + /// orientation or motion of the airframe. Calibration should be + /// as for the previous COLD_START call. + /// + /// @param style The initialisation startup style. + /// + virtual void init(Start_style style) = 0; + + /// Perform 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; + + /// 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; + + /// Give the IMU some cycles to perform/fetch an update from its + /// sensors. + /// + /// @returns True if some state was updated. + /// + virtual bool update(void) = 0; + + /// Fetch the current gyro values + /// + /// @returns vector of rotational rates in radians/sec + /// + Vector3f get_gyro(void) { return _gyro; } + + /// Fetch the current accelerometer values + /// + /// @returns vector of current accelerations in m/s/s + /// + Vector3f get_accel(void) { return _accel; } + + /// A count of bad sensor readings + /// + /// @todo This should be renamed, as there's no guarantee that sensors + /// are using ADCs, etc. + /// + 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; + + /// Most recent gyro reading obtained by ::update + Vector3f _gyro; +}; + +#endif diff --git a/libraries/AP_IMU/IMU_Shim.cpp b/libraries/AP_IMU/IMU_Shim.cpp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde b/libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde new file mode 100644 index 0000000000..15ffe7c634 --- /dev/null +++ b/libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde @@ -0,0 +1,38 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +// +// Simple test for the AP_IMU driver. +// + +#include +#include +#include +#include +#include + +FastSerialPort(Serial, 0); + +AP_ADC_ADS7844 adc; +AP_IMU_Oilpan imu(&adc, 0); // disable warm-start for now + +void setup(void) +{ + Serial.begin(38400); + Serial.println("Doing IMU startup..."); + adc.Init(); + imu.init(IMU::COLD_START); +} + +void loop(void) +{ + Vector3f accel; + Vector3f gyro; + + delay(1000); + imu.update(); + accel = imu.get_accel(); + gyro = imu.get_gyro(); + + Serial.printf("AX: 0x%4.4f AY: 0x%4.4f AZ: 0x%4.4f GX: 0x%4.4f GY: 0x%4.4f GZ: 0x%4.4f\n", + accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z); +}