zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Implement an abstract IMU class. 

Refactor the existing AP_IMU as AP_IMU_Oilpan (this will require changes in other projects TBD).

Add a shim IMU class for use by e.g. HIL protocol handlers.

This paves the way for a better handling of HIL_MODE_SENSORS as well as the mooted SPI-based oilpan IMU.



git-svn-id: https://arducopter.googlecode.com/svn/trunk@1342 f9c3cf11-9bcb-44bc-f272-b75c42450872
mission-4.1.18
DrZiplok 14 years ago
parent
632d0f574a
commit
94fd2431e4
8 changed files with 591 additions and 401 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 333
      libraries/AP_IMU/AP_IMU.cpp
  2. 73
      libraries/AP_IMU/AP_IMU.h
  3. 378
      libraries/AP_IMU/AP_IMU_Oilpan.cpp
  4. 62
      libraries/AP_IMU/AP_IMU_Oilpan.h
  5. 12
      libraries/AP_IMU/AP_IMU_Shim.h
  6. 96
      libraries/AP_IMU/IMU.h
  7. 0
      libraries/AP_IMU/IMU_Shim.cpp
  8. 38
      libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde

333
libraries/AP_IMU/AP_IMU.cpp
Unescape View File

@ -1,333 +0,0 @@ @@ -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 <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.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]);
}

73
libraries/AP_IMU/AP_IMU.h
Unescape View File

@ -1,70 +1,7 @@ @@ -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 <FastSerial.h>
#include <AP_Math.h>
#include <inttypes.h>
#include "WProgram.h"
#include <AP_ADC.h>
#include <avr/eeprom.h>
/// @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"

378
libraries/AP_IMU/AP_IMU_Oilpan.cpp
Unescape View File

@ -0,0 +1,378 @@ @@ -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 <FastSerial.h>
#include <AP_Common.h>
#include <avr/eeprom.h>
#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]);
}

62
libraries/AP_IMU/AP_IMU_Oilpan.h
Unescape View File

@ -0,0 +1,62 @@ @@ -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 <AP_Math.h>
#include <AP_ADC.h>
#include <inttypes.h>
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

12
libraries/AP_IMU/AP_IMU_Shim.h
Unescape View File

@ -0,0 +1,12 @@ @@ -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

96
libraries/AP_IMU/IMU.h
Unescape View File

@ -0,0 +1,96 @@ @@ -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 <AP_Math.h>
#include <inttypes.h>
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

0
libraries/AP_IMU/IMU_Shim.cpp
Unescape View File

38
libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde
Unescape View File

@ -0,0 +1,38 @@ @@ -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 <FastSerial.h>
#include <AP_IMU.h>
#include <AP_ADC.h>
#include <AP_Math.h>
#include <AP_Common.h>
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);
}
Write Preview
Loading…
Cancel
Save