mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
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
This commit is contained in:
DrZiplok
parent
1dabb6c9ce
commit
c1d97878a5
@ -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]);
|
||||
}
|
||||
|
||||
@ -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
Normal file
378
libraries/AP_IMU/AP_IMU_Oilpan.cpp
Normal file
@ -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
Normal file
62
libraries/AP_IMU/AP_IMU_Oilpan.h
Normal file
@ -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
Normal file
12
libraries/AP_IMU/AP_IMU_Shim.h
Normal file
@ -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
Normal file
96
libraries/AP_IMU/IMU.h
Normal file
@ -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
Normal file
0
libraries/AP_IMU/IMU_Shim.cpp
Normal file
38
libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde
Normal file
38
libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde
Normal file
@ -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);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user