Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

purple: rework AP_IMU library to use AP_IntertialSensor library

Browse Source 
the AP_IMU keeps the handling of the calibration, but most of the real
work happens in the lower level AP_IntertialSensor library
This commit is contained in:
Pat Hickey 2011-11-13 14:24:03 +11:00
parent db8708911a
commit 433caaedc0
14 changed files with 516 additions and 422 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_IMU/AP_IMU.h
View File

@ -3,5 +3,6 @@
/// @file AP_IMU.h /// @file AP_IMU.h
/// @brief Catch-all header that defines all supported IMU classes. /// @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_Shim.h"
#include "AP_IMU_INS.h"

250
libraries/AP_IMU/AP_IMU_INS.cpp Normal file
View File

@ -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 Normal file
View File

@ -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
View File

@ -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
View File

@ -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
View File

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

34
libraries/AP_IMU/IMU.cpp Normal file
View File

@ -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
View File

@ -8,6 +8,7 @@
#define IMU_h #define IMU_h
#include "../AP_Math/AP_Math.h" #include "../AP_Math/AP_Math.h"
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
#include <inttypes.h> #include <inttypes.h>
class IMU class IMU
@ -15,7 +16,7 @@ class IMU
public: public:
/// Constructor /// Constructor
IMU() {} IMU();
enum Start_style { enum Start_style {
COLD_START = 0, COLD_START = 0,
@ -35,28 +36,30 @@ public:
/// ///
/// @param style The initialisation startup style. /// @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. /// Perform cold startup initialisation for just the accelerometers.
/// ///
/// @note This should not be called unless ::init has previously /// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work. /// 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. /// Perform cold-start initialisation for just the gyros.
/// ///
/// @note This should not be called unless ::init has previously /// @note This should not be called unless ::init has previously
/// been called, as ::init may perform other work /// 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 /// Give the IMU some cycles to perform/fetch an update from its
/// sensors. /// sensors.
/// ///
/// @returns True if some state was updated. /// @returns True if some state was updated.
/// ///
virtual bool update(void) = 0; virtual bool update(void);
/// Fetch the current gyro values /// Fetch the current gyro values
/// ///
@ -90,6 +93,16 @@ public:
/// ///
uint8_t adc_constraints; 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: protected:
/// Most recent accelerometer reading obtained by ::update /// Most recent accelerometer reading obtained by ::update
Vector3f _accel; Vector3f _accel;

55
libraries/AP_IMU/examples/AP_IMU_MPU6000/AP_IMU_MPU6000.pde Normal file
View File

@ -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
View File

50
libraries/AP_IMU/examples/IMU_MPU6000_test/IMU_MPU6000_test.pde Normal file
View File

@ -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 Normal file
View File

@ -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
View File

@ -1,10 +1,14 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
// //
// Simple test for the AP_IMU driver. // Simple test for the AP_IMU_Oilpan driver.
// //
#include <FastSerial.h> #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_IMU.h>
#include <AP_ADC.h> #include <AP_ADC.h>
#include <AP_Math.h> #include <AP_Math.h>
@ -12,15 +16,23 @@
FastSerialPort(Serial, 0); FastSerialPort(Serial, 0);
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerAperiodicProcess adc_scheduler;
AP_ADC_ADS7844 adc; 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) void setup(void)
{ {
Serial.begin(38400); Serial.begin(115200);
Serial.println("Doing IMU startup..."); 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) void loop(void)

2
libraries/AP_IMU/examples/IMU_Oilpan_test/Makefile Normal file
View File

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