zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

purple: rework AP_IMU library to use AP_IntertialSensor library 

the AP_IMU keeps the handling of the calibration, but most of the real
work happens in the lower level AP_IntertialSensor library
master
Pat Hickey 13 years ago
parent
82b48784ef
commit
e53bcba4d9
14 changed files with 516 additions and 422 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. 3
      libraries/AP_IMU/AP_IMU.h
  2. 250
      libraries/AP_IMU/AP_IMU_INS.cpp
  3. 84
      libraries/AP_IMU/AP_IMU_INS.h
  4. 302
      libraries/AP_IMU/AP_IMU_Oilpan.cpp
  5. 109
      libraries/AP_IMU/AP_IMU_Oilpan.h
  6. 2
      libraries/AP_IMU/AP_IMU_Shim.h
  7. 34
      libraries/AP_IMU/IMU.cpp
  8. 23
      libraries/AP_IMU/IMU.h
  9. 55
      libraries/AP_IMU/examples/AP_IMU_MPU6000/AP_IMU_MPU6000.pde
  10. 0
      libraries/AP_IMU/examples/AP_IMU_MPU6000/Makefile
  11. 50
      libraries/AP_IMU/examples/IMU_MPU6000_test/IMU_MPU6000_test.pde
  12. 2
      libraries/AP_IMU/examples/IMU_MPU6000_test/Makefile
  13. 22
      libraries/AP_IMU/examples/IMU_Oilpan_test/IMU_Oilpan_test.pde
  14. 2
      libraries/AP_IMU/examples/IMU_Oilpan_test/Makefile

3
libraries/AP_IMU/AP_IMU.h
Unescape View File

@ -3,5 +3,6 @@ @@ -3,5 +3,6 @@
/// @file AP_IMU.h
/// @brief Catch-all header that defines all supported IMU classes.
#include "AP_IMU_Oilpan.h"
#include "IMU.h"
#include "AP_IMU_Shim.h"
#include "AP_IMU_INS.h"

250
libraries/AP_IMU/AP_IMU_INS.cpp
Unescape View File

@ -0,0 +1,250 @@ @@ -0,0 +1,250 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
//
// AP_IMU_INS.cpp - IMU Sensor Library for Ardupilot Mega
// Code by Michael Smith, Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com
//
// 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_INS.cpp
/// @brief IMU driver on top of an INS driver. Provides calibration for the
// inertial sensors (gyro and accel)
#include <FastSerial.h>
#include <AP_Common.h>
#include <avr/eeprom.h>
#include "AP_IMU_INS.h"
// XXX secret knowledge about the APM/oilpan wiring
//
#define A_LED_PIN 37
#define C_LED_PIN 35
void
AP_IMU_INS::init( Start_style style,
void (*delay_cb)(unsigned long t),
AP_PeriodicProcess * scheduler )
{
_ins->init(scheduler);
// 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(delay_cb);
_init_accel(delay_cb);
// save calibration
_sensor_cal.save();
}
}
/**************************************************/
void
AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t))
{
_init_gyro(delay_cb);
_sensor_cal.save();
}
void
AP_IMU_INS::_init_gyro(void (*delay_cb)(unsigned long t))
{
int flashcount = 0;
float adc_in;
float prev[3] = {0,0,0};
float total_change;
float max_offset;
float ins_gyro[6];
// cold start
delay_cb(500);
Serial.printf_P(PSTR("Init Gyro"));
for(int c = 0; c < 25; c++){
// Mostly we are just flashing the LED's here
// to tell the user to keep the IMU still
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH);
delay_cb(20);
_ins->update();
_ins->get_gyros(ins_gyro);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, LOW);
delay_cb(20);
}
for (int j = 0; j <= 2; j++)
_sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
_ins->update();
_ins->get_gyros(ins_gyro);
for (int j = 0; j <= 2; j++){
prev[j] = _sensor_cal[j];
adc_in = ins_gyro[j];
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){
_ins->update();
_ins->get_gyros(ins_gyro);
for (int j = 0; j < 3; j++){
adc_in = ins_gyro[j];
// filter
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
delay_cb(20);
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
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++;
}
total_change = fabs(prev[0] - _sensor_cal[0]) + fabs(prev[1] - _sensor_cal[1]) +fabs(prev[2] - _sensor_cal[2]);
max_offset = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1];
max_offset = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2];
delay_cb(500);
} while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
}
void
AP_IMU_INS::save()
{
_sensor_cal.save();
}
void
AP_IMU_INS::init_accel(void (*delay_cb)(unsigned long t))
{
_init_accel(delay_cb);
_sensor_cal.save();
}
void
AP_IMU_INS::_init_accel(void (*delay_cb)(unsigned long t))
{
int flashcount = 0;
float adc_in;
float prev[6] = {0,0,0};
float total_change;
float max_offset;
float ins_accel[3];
// cold start
delay_cb(500);
Serial.printf_P(PSTR("Init Accel"));
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
_ins->update();
_ins->get_accels(ins_accel);
for (int j = 3; j <= 5; j++){
prev[j] = _sensor_cal[j];
adc_in = ins_accel[j-3];
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){ // We take some readings...
delay_cb(20);
_ins->update();
_ins->get_accels(ins_accel);
for (int j = 3; j < 6; j++){
adc_in = ins_accel[j-3];
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
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++;
}
// null gravity from the Z accel
_sensor_cal[5] += 9.805;
total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]);
max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
delay_cb(500);
} while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
Serial.printf_P(PSTR(" "));
}
float
AP_IMU_INS::_calibrated(uint8_t channel, float ins_value)
{
return ins_value - _sensor_cal[channel];
}
bool
AP_IMU_INS::update(void)
{
float gyros[3];
float accels[3];
_ins->update();
_ins->get_gyros(gyros);
_ins->get_accels(accels);
_sample_time = _ins->sample_time();
// convert corrected gyro readings to delta acceleration
//
_gyro.x = _calibrated(0, gyros[0]);
_gyro.y = _calibrated(1, gyros[1]);
_gyro.z = _calibrated(2, gyros[2]);
// convert corrected accelerometer readings to acceleration
//
_accel.x = _calibrated(3, accels[0]);
_accel.y = _calibrated(4, accels[1]);
_accel.z = _calibrated(5, accels[2]);
_accel_filtered.x = _accel_filtered.x / 2 + _accel.x / 2;
_accel_filtered.y = _accel_filtered.y / 2 + _accel.y / 2;
_accel_filtered.z = _accel_filtered.z / 2 + _accel.z / 2;
// always updated
return true;
}

84
libraries/AP_IMU/AP_IMU_INS.h
Unescape View File

@ -0,0 +1,84 @@ @@ -0,0 +1,84 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AP_IMU_INS.h
/// @brief IMU driver on top of an AP_InertialSensor (INS) driver.
// Provides offset calibration for for the gyro and accel.
#ifndef __AP_IMU_INS_H__
#define __AP_IMU_INS_H__
#include "../AP_Common/AP_Common.h"
#include "../AP_Math/AP_Math.h"
#include "../AP_InertialSensor/AP_InertialSensor.h"
#include <inttypes.h>
#include "IMU.h"
class AP_IMU_INS : public IMU
{
public:
/// 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_INS(AP_InertialSensor *ins, AP_Var::Key key) :
_ins(ins),
_sensor_cal(key, PSTR("IMU_SENSOR_CAL"))
{}
/// 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,
void (*delay_cb)(unsigned long t) = delay,
AP_PeriodicProcess *scheduler = NULL );
virtual void save();
virtual void init_accel(void (*delay_cb)(unsigned long t) = delay);
virtual void init_gyro(void (*delay_cb)(unsigned long t) = delay);
virtual bool update(void);
// for jason
virtual float gx() { return _sensor_cal[0]; }
virtual float gy() { return _sensor_cal[1]; }
virtual float gz() { return _sensor_cal[2]; }
virtual float ax() { return _sensor_cal[3]; }
virtual float ay() { return _sensor_cal[4]; }
virtual float az() { return _sensor_cal[5]; }
virtual void ax(const float v) { _sensor_cal[3] = v; }
virtual void ay(const float v) { _sensor_cal[4] = v; }
virtual void az(const float v) { _sensor_cal[5] = v; }
private:
AP_InertialSensor *_ins; ///< INS provides an axis and unit correct sensor source.
AP_VarA<float,6> _sensor_cal; ///< Calibrated sensor offsets
virtual void _init_accel(void (*delay_cb)(unsigned long t)); ///< no-save implementation
virtual void _init_gyro(void (*delay_cb)(unsigned long t)); ///< no-save implementation
float _calibrated(uint8_t channel, float ins_value);
// Gyro and Accelerometer calibration criterial
//
static const float _gyro_total_cal_change = 4.0; // Experimentally derived - allows for some minor motion
static const float _gyro_max_cal_offset = 320.0;
static const float _accel_total_cal_change = 4.0;
static const float _accel_max_cal_offset = 250.0;
};
#endif

302
libraries/AP_IMU/AP_IMU_Oilpan.cpp
Unescape View File

@ -1,302 +0,0 @@ @@ -1,302 +0,0 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
//
// AP_IMU.cpp - IMU Sensor Library for Ardupilot Mega
// Code by Michael Smith, 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"
// 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}; // 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] = {
{1658,0,0}, // Values to use if no temp compensation data available
{1658,0,0}, // Based on average values for 20 sample boards
{1658,0,0}
};
void
AP_IMU_Oilpan::init(Start_style style, void (*callback)(unsigned long t))
{
// 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(callback);
_init_accel(callback);
// save calibration
_sensor_cal.save();
}
}
/**************************************************/
void
AP_IMU_Oilpan::init_gyro(void (*callback)(unsigned long t))
{
_init_gyro(callback);
_sensor_cal.save();
}
void
AP_IMU_Oilpan::_init_gyro(void (*callback)(unsigned long t))
{
int flashcount = 0;
int tc_temp;
float adc_in;
float prev[3] = {0,0,0};
float total_change;
float max_offset;
uint16_t adc_values[6];
// cold start
tc_temp = _adc->Ch(_gyro_temp_ch);
callback(500);
Serial.printf_P(PSTR("Init Gyro"));
for(int c = 0; c < 25; c++){ // Mostly we are just flashing the LED's here to tell the user to keep the IMU still
digitalWrite(A_LED_PIN, LOW);
digitalWrite(C_LED_PIN, HIGH);
callback(20);
_adc->Ch6(_sensors, adc_values);
digitalWrite(A_LED_PIN, HIGH);
digitalWrite(C_LED_PIN, LOW);
callback(20);
}
for (int j = 0; j <= 2; j++)
_sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
// get 6 sensor values
_adc->Ch6(_sensors, adc_values);
for (int j = 0; j <= 2; j++){
prev[j] = _sensor_cal[j];
adc_in = adc_values[j];
adc_in -= _sensor_compensation(j, tc_temp);
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){
// get 6 sensor values
_adc->Ch6(_sensors, adc_values);
for (int j = 0; j < 3; j++){
adc_in = adc_values[j];
// Subtract temp compensated typical gyro bias
adc_in -= _sensor_compensation(j, tc_temp);
// filter
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
callback(20);
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
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++;
}
total_change = fabs(prev[0] - _sensor_cal[0]) + fabs(prev[1] - _sensor_cal[1]) +fabs(prev[2] - _sensor_cal[2]);
max_offset = (_sensor_cal[0] > _sensor_cal[1]) ? _sensor_cal[0] : _sensor_cal[1];
max_offset = (max_offset > _sensor_cal[2]) ? max_offset : _sensor_cal[2];
callback(500);
} while ( total_change > _gyro_total_cal_change || max_offset > _gyro_max_cal_offset);
}
void
AP_IMU_Oilpan::save()
{
_sensor_cal.save();
}
void
AP_IMU_Oilpan::init_accel(void (*callback)(unsigned long t))
{
_init_accel(callback);
_sensor_cal.save();
}
void
AP_IMU_Oilpan::_init_accel(void (*callback)(unsigned long t))
{
int flashcount = 0;
float adc_in;
float prev[6] = {0,0,0};
float total_change;
float max_offset;
uint16_t adc_values[6];
// cold start
callback(500);
Serial.printf_P(PSTR("Init Accel"));
for (int j=3; j<=5; j++) _sensor_cal[j] = 500; // Just a large value to load prev[j] the first time
do {
_adc->Ch6(_sensors, adc_values);
for (int j = 3; j <= 5; j++){
prev[j] = _sensor_cal[j];
adc_in = adc_values[j];
adc_in -= _sensor_compensation(j, 0); // temperature ignored
_sensor_cal[j] = adc_in;
}
for(int i = 0; i < 50; i++){ // We take some readings...
callback(20);
_adc->Ch6(_sensors, adc_values);
for (int j = 3; j < 6; j++){
adc_in = adc_values[j];
adc_in -= _sensor_compensation(j, 0); // temperature ignored
_sensor_cal[j] = _sensor_cal[j] * 0.9 + adc_in * 0.1;
}
if(flashcount == 5) {
Serial.printf_P(PSTR("*"));
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++;
}
// null gravity from the Z accel
_sensor_cal[5] += _gravity * _sensor_signs[5];
total_change = fabs(prev[3] - _sensor_cal[3]) + fabs(prev[4] - _sensor_cal[4]) +fabs(prev[5] - _sensor_cal[5]);
max_offset = (_sensor_cal[3] > _sensor_cal[4]) ? _sensor_cal[3] : _sensor_cal[4];
max_offset = (max_offset > _sensor_cal[5]) ? max_offset : _sensor_cal[5];
callback(500);
} while ( total_change > _accel_total_cal_change || max_offset > _accel_max_cal_offset);
Serial.printf_P(PSTR(" "));
}
/**************************************************/
// Returns the temperature compensated raw gyro value
//---------------------------------------------------
float
AP_IMU_Oilpan::_sensor_compensation(uint8_t channel, int temperature) const
{
// do gyro temperature compensation
if (channel < 3) {
return 1658.0;
/*
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 2041.0; // Average raw value from a 20 board sample
}
float
AP_IMU_Oilpan::_sensor_in(uint8_t channel, uint16_t adc_value, int temperature)
{
float adc_in;
// get the compensated sensor value
//
adc_in = adc_value - _sensor_compensation(channel, temperature);
// 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;
}
bool
AP_IMU_Oilpan::update(void)
{
int tc_temp = _adc->Ch(_gyro_temp_ch);
uint16_t adc_values[6];
_sample_time = _adc->Ch6(_sensors, adc_values);
// convert corrected gyro readings to delta acceleration
//
_gyro.x = _gyro_gain_x * _sensor_in(0, adc_values[0], tc_temp);
_gyro.y = _gyro_gain_y * _sensor_in(1, adc_values[1], tc_temp);
_gyro.z = _gyro_gain_z * _sensor_in(2, adc_values[2], tc_temp);
// convert corrected accelerometer readings to acceleration
//
_accel.x = _accel_scale * _sensor_in(3, adc_values[3], tc_temp);
_accel.y = _accel_scale * _sensor_in(4, adc_values[4], tc_temp);
_accel.z = _accel_scale * _sensor_in(5, adc_values[5], tc_temp);
_accel_filtered.x = _accel_filtered.x / 2 + _accel.x / 2;
_accel_filtered.y = _accel_filtered.y / 2 + _accel.y / 2;
_accel_filtered.z = _accel_filtered.z / 2 + _accel.z / 2;
// always updated
return true;
}

109
libraries/AP_IMU/AP_IMU_Oilpan.h
Unescape View File

@ -1,109 +0,0 @@ @@ -1,109 +0,0 @@
// -*- 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
#ifndef AP_IMU_Oilpan_h
#define AP_IMU_Oilpan_h
#include "../AP_Common/AP_Common.h"
#include "../AP_Math/AP_Math.h"
#include "../AP_ADC/AP_ADC.h"
#include <inttypes.h>
#include "IMU.h"
class AP_IMU_Oilpan : public IMU
{
public:
/// 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"))
// _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, void (*callback)(unsigned long t) = delay);
virtual void save();
virtual void init_accel(void (*callback)(unsigned long t) = delay);
virtual void init_gyro(void (*callback)(unsigned long t) = delay);
virtual bool update(void);
// for jason
float gx() { return _sensor_cal[0]; }
float gy() { return _sensor_cal[1]; }
float gz() { return _sensor_cal[2]; }
float ax() { return _sensor_cal[3]; }
float ay() { return _sensor_cal[4]; }
float az() { return _sensor_cal[5]; }
void ax(const float v) { _sensor_cal[3] = v; }
void ay(const float v) { _sensor_cal[4] = v; }
void az(const float v) { _sensor_cal[5] = v; }
private:
AP_ADC *_adc; ///< ADC that we use for reading sensors
AP_VarA<float,6> _sensor_cal; ///< Calibrated sensor offsets
virtual void _init_accel(void (*callback)(unsigned long t)); ///< no-save implementation
virtual void _init_gyro(void (*callback)(unsigned long t)); ///< no-save implementation
float _sensor_in(uint8_t channel, uint16_t adc_value, int temperature);
float _sensor_compensation(uint8_t channel, int temp) const;
// constants
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 = 423.8; ///< 1G in the raw data coming from the accelerometer
// Value based on actual sample data from 20 boards
static const float _accel_scale = 9.80665 / 423.8; ///< 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 = ToRad(0.4); // X axis Gyro gain
static const float _gyro_gain_y = ToRad(0.41); // Y axis Gyro gain
static const float _gyro_gain_z = ToRad(0.41); // Z axis Gyro gain
// Maximum possible value returned by an offset-corrected sensor channel
//
static const float _adc_constraint = 900;
// Gyro and Accelerometer calibration criterial
//
static const float _gyro_total_cal_change = 4.0; // Experimentally derived - allows for some minor motion
static const float _gyro_max_cal_offset = 320.0;
static const float _accel_total_cal_change = 4.0;
static const float _accel_max_cal_offset = 250.0;
};
#endif

2
libraries/AP_IMU/AP_IMU_Shim.h
Unescape View File

@ -6,6 +6,8 @@ @@ -6,6 +6,8 @@
#ifndef AP_IMU_Shim_h
#define AP_IMU_Shim_h
#include "IMU.h"
class AP_IMU_Shim : public IMU
{
public:

34
libraries/AP_IMU/IMU.cpp
Unescape View File

@ -0,0 +1,34 @@ @@ -0,0 +1,34 @@
#include "IMU.h"
/* Empty implementations for the IMU functions.
* Although these will never be used, in certain situations with
* optimizations turned off, having empty implementations in an object
* file will help satisify the linker.
*/
IMU::IMU () {}
void IMU::init( Start_style style,
void (*delay_cb)(unsigned long t),
AP_PeriodicProcess * scheduler )
{ }
void IMU::init_accel(void (*delay_cb)(unsigned long t))
{ }
void IMU::init_gyro(void (*delay_cb)(unsigned long t))
{ }
bool IMU::update(void) { return false; }
float IMU::gx(void) { return 0.0; }
float IMU::gy(void) { return 0.0; }
float IMU::gz(void) { return 0.0; }
float IMU::ax(void) { return 0.0; }
float IMU::ay(void) { return 0.0; }
float IMU::az(void) { return 0.0; }
void IMU::ax(const float v) { }
void IMU::ay(const float v) { }
void IMU::az(const float v) { }

23
libraries/AP_IMU/IMU.h
Unescape View File

@ -8,6 +8,7 @@ @@ -8,6 +8,7 @@
#define IMU_h
#include "../AP_Math/AP_Math.h"
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
#include <inttypes.h>
class IMU
@ -15,7 +16,7 @@ class IMU @@ -15,7 +16,7 @@ class IMU
public:
/// Constructor
IMU() {}
IMU();
enum Start_style {
COLD_START = 0,
@ -35,28 +36,30 @@ public: @@ -35,28 +36,30 @@ public:
///
/// @param style The initialisation startup style.
///
virtual void init(Start_style style, void (*callback)(unsigned long t)) = 0;
virtual void init( Start_style style,
void (*delay_cb)(unsigned long t),
AP_PeriodicProcess * scheduler );
/// 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.
///
virtual void init_accel(void (*callback)(unsigned long t)) = 0;
virtual void init_accel(void (*callback)(unsigned long t));
/// 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
///
virtual void init_gyro(void (*callback)(unsigned long t)) = 0;
virtual void init_gyro(void (*callback)(unsigned long t));
/// 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;
virtual bool update(void);
/// Fetch the current gyro values
///
@ -90,6 +93,16 @@ public: @@ -90,6 +93,16 @@ public:
///
uint8_t adc_constraints;
virtual float gx(void);
virtual float gy(void);
virtual float gz(void);
virtual float ax(void);
virtual float ay(void);
virtual float az(void);
virtual void ax(const float v);
virtual void ay(const float v);
virtual void az(const float v);
protected:
/// Most recent accelerometer reading obtained by ::update
Vector3f _accel;

55
libraries/AP_IMU/examples/AP_IMU_MPU6000/AP_IMU_MPU6000.pde
Unescape View File

@ -0,0 +1,55 @@ @@ -0,0 +1,55 @@
// -*- 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_IMU_MPU6000.h> // Experimental MPU6000 IMU library
#include <AP_PeriodicProcess.h> // Parent header of Timer and TimerAperiodic
// (only included for makefile libpath to work)
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
#include <AP_TimerAperiodicProcess.h> // TimerAperiodicProcess is the scheduler for ADC reads.
#include <AP_ADC.h>
#include <AP_Math.h>
#include <AP_Common.h>
#include <SPI.h>
FastSerialPort(Serial, 0);
#ifndef CONFIG_MPU6000_CHIP_SELECT_PIN
# define CONFIG_MPU6000_CHIP_SELECT_PIN 53
#endif
AP_IMU_MPU6000 imu(140,
CONFIG_MPU6000_CHIP_SELECT_PIN);
AP_TimerProcess timer_scheduler;
AP_ADC_ADS7844 adc;
void setup(void)
{
Serial.begin(115200);
Serial.println("Doing IMU startup...");
timer_scheduler.init();
Serial.println("done timer init");
adc.Init(&timer_scheduler);
Serial.println("done adc init");
imu.init(IMU::COLD_START, delay, &timer_scheduler);
Serial.println("done IMU init");
delay(1000);
}
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);
}

0
libraries/AP_IMU/examples/AP_IMU/Makefile → libraries/AP_IMU/examples/AP_IMU_MPU6000/Makefile
Unescape View File

50
libraries/AP_IMU/examples/IMU_MPU6000_test/IMU_MPU6000_test.pde
Unescape View File

@ -0,0 +1,50 @@ @@ -0,0 +1,50 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// Simple test for the AP_IMU_Oilpan driver.
//
#include <FastSerial.h>
#include <SPI.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h>
#include <AP_IMU.h>
#include <AP_Math.h>
#include <AP_Common.h>
FastSerialPort(Serial, 0);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess mpu_scheduler;
AP_InertialSensor_MPU6000 mpu6000( 53 ); /* chip select is pin 53 */
AP_IMU_INS imu(&mpu6000, 0); /* second arg is for Parameters. Can we leave it null?*/
void setup(void)
{
pinMode(53, OUTPUT);
digitalWrite(53, HIGH);
Serial.begin(115200);
Serial.println("Doing IMU startup...");
isr_registry.init();
mpu_scheduler.init(&isr_registry);
imu.init(IMU::COLD_START, delay, &mpu_scheduler);
}
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);
}

2
libraries/AP_IMU/examples/IMU_MPU6000_test/Makefile
Unescape View File

@ -0,0 +1,2 @@ @@ -0,0 +1,2 @@
BOARD = mega2560
include ../../../AP_Common/Arduino.mk

22
libraries/AP_IMU/examples/AP_IMU/AP_IMU.pde → libraries/AP_IMU/examples/IMU_Oilpan_test/IMU_Oilpan_test.pde
Unescape View File

@ -1,10 +1,14 @@ @@ -1,10 +1,14 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
//
// Simple test for the AP_IMU driver.
// Simple test for the AP_IMU_Oilpan driver.
//
#include <FastSerial.h>
#include <SPI.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h>
#include <AP_IMU.h>
#include <AP_ADC.h>
#include <AP_Math.h>
@ -12,15 +16,23 @@ @@ -12,15 +16,23 @@
FastSerialPort(Serial, 0);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerAperiodicProcess adc_scheduler;
AP_ADC_ADS7844 adc;
AP_IMU_Oilpan imu(&adc, 0); // disable warm-start for now
AP_InertialSensor_Oilpan oilpan_ins(&adc);
AP_IMU_INS imu(&oilpan_ins,0);
void setup(void)
{
Serial.begin(38400);
Serial.begin(115200);
Serial.println("Doing IMU startup...");
adc.Init();
imu.init(IMU::COLD_START);
isr_registry.init();
adc_scheduler.init(&isr_registry);
/* Should also call ins.init and adc.init */
imu.init(IMU::COLD_START, delay, &adc_scheduler);
}
void loop(void)

2
libraries/AP_IMU/examples/IMU_Oilpan_test/Makefile
Unescape View File

@ -0,0 +1,2 @@ @@ -0,0 +1,2 @@
BOARD = mega2560
include ../../../AP_Common/Arduino.mk
Write Preview
Loading…
Cancel
Save