AP_InertialSensor: merge in calibration features from IMU library
add gauss-newton method of accelerometer calibration
This commit is contained in:
rmackay9
parent
8652bfee8d
commit
49de46a548
463
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Normal file
463
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Normal file
@ -0,0 +1,463 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
#include <FastSerial.h>
|
||||
|
||||
#include "AP_InertialSensor.h"
|
||||
|
||||
#include <SPI.h>
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include <wiring.h>
|
||||
#endif
|
||||
|
||||
#define FLASH_LEDS(on) do { if (flash_leds_cb != NULL) flash_leds_cb(on); } while (0)
|
||||
|
||||
#define SAMPLE_UNIT 1
|
||||
|
||||
// Class level parameters
|
||||
const AP_Param::GroupInfo AP_InertialSensor::var_info[] PROGMEM = {
|
||||
AP_GROUPINFO("PRODUCT_ID", 0, AP_InertialSensor, _product_id, 0),
|
||||
AP_GROUPINFO("ACCSCA", 1, AP_InertialSensor, _accel_scale, 0),
|
||||
AP_GROUPINFO("ACCOFF", 2, AP_InertialSensor, _accel_offset, 0),
|
||||
AP_GROUPINFO("GYROFF", 3, AP_InertialSensor, _gyro_offset, 0),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_InertialSensor::AP_InertialSensor()
|
||||
{
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::init( Start_style style,
|
||||
void (*delay_cb)(unsigned long t),
|
||||
void (*flash_leds_cb)(bool on),
|
||||
AP_PeriodicProcess * scheduler )
|
||||
{
|
||||
_product_id = _init(scheduler);
|
||||
|
||||
// check scaling
|
||||
Vector3f accel_scale = _accel_scale.get();
|
||||
if( accel_scale.x == 0 && accel_scale.y == 0 && accel_scale.z == 0 ) {
|
||||
accel_scale.x = accel_scale.y = accel_scale.z = 1.0;
|
||||
_accel_scale.set(accel_scale);
|
||||
}
|
||||
|
||||
// if we are warm-starting, load the calibration data from EEPROM and go
|
||||
if (WARM_START == style) {
|
||||
load_parameters();
|
||||
} else {
|
||||
|
||||
// do cold-start calibration for both accel and gyro
|
||||
_init_gyro(delay_cb, flash_leds_cb);
|
||||
|
||||
// save calibration
|
||||
save_parameters();
|
||||
}
|
||||
}
|
||||
|
||||
// save parameters to eeprom
|
||||
void AP_InertialSensor::load_parameters()
|
||||
{
|
||||
_product_id.load();
|
||||
_accel_scale.load();
|
||||
_accel_offset.load();
|
||||
}
|
||||
|
||||
// save parameters to eeprom
|
||||
void AP_InertialSensor::save_parameters()
|
||||
{
|
||||
_product_id.save();
|
||||
_accel_scale.save();
|
||||
_accel_offset.save();
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
|
||||
{
|
||||
_init_gyro(delay_cb, flash_leds_cb);
|
||||
|
||||
// save calibration
|
||||
save_parameters();
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::_init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
|
||||
{
|
||||
Vector3f last_average, best_avg;
|
||||
Vector3f ins_gyro;
|
||||
float best_diff = 0;
|
||||
|
||||
// cold start
|
||||
delay_cb(100);
|
||||
Serial.printf_P(PSTR("Init Gyro"));
|
||||
|
||||
// remove existing gyro offsets
|
||||
_gyro_offset = Vector3f(0,0,0);
|
||||
|
||||
for(int16_t c = 0; c < 25; c++) {
|
||||
// Mostly we are just flashing the LED's here
|
||||
// to tell the user to keep the IMU still
|
||||
FLASH_LEDS(true);
|
||||
delay_cb(20);
|
||||
|
||||
update();
|
||||
ins_gyro = get_gyro();
|
||||
|
||||
FLASH_LEDS(false);
|
||||
delay_cb(20);
|
||||
}
|
||||
|
||||
// the strategy is to average 200 points over 1 second, then do it
|
||||
// again and see if the 2nd average is within a small margin of
|
||||
// the first
|
||||
|
||||
last_average.zero();
|
||||
|
||||
// we try to get a good calibration estimate for up to 10 seconds
|
||||
// if the gyros are stable, we should get it in 2 seconds
|
||||
for (int16_t j = 0; j <= 10; j++) {
|
||||
Vector3f gyro_sum, gyro_avg, gyro_diff;
|
||||
float diff_norm;
|
||||
uint8_t i;
|
||||
|
||||
Serial.printf_P(PSTR("*"));
|
||||
|
||||
gyro_sum.zero();
|
||||
for (i=0; i<200; i++) {
|
||||
update();
|
||||
ins_gyro = get_gyro();
|
||||
gyro_sum += ins_gyro;
|
||||
if (i % 40 == 20) {
|
||||
FLASH_LEDS(true);
|
||||
} else if (i % 40 == 0) {
|
||||
FLASH_LEDS(false);
|
||||
}
|
||||
delay_cb(5);
|
||||
}
|
||||
gyro_avg = gyro_sum / i;
|
||||
|
||||
gyro_diff = last_average - gyro_avg;
|
||||
diff_norm = gyro_diff.length();
|
||||
|
||||
if (j == 0) {
|
||||
best_diff = diff_norm;
|
||||
best_avg = gyro_avg;
|
||||
} else if (gyro_diff.length() < ToRad(0.04)) {
|
||||
// we want the average to be within 0.1 bit, which is 0.04 degrees/s
|
||||
last_average = (gyro_avg * 0.5) + (last_average * 0.5);
|
||||
_gyro_offset = last_average;
|
||||
|
||||
// all done
|
||||
return;
|
||||
} else if (diff_norm < best_diff) {
|
||||
best_diff = diff_norm;
|
||||
best_avg = (gyro_avg * 0.5) + (last_average * 0.5);
|
||||
}
|
||||
last_average = gyro_avg;
|
||||
}
|
||||
|
||||
// we've kept the user waiting long enough - use the best pair we
|
||||
// found so far
|
||||
Serial.printf_P(PSTR("\ngyro did not converge: diff=%f dps\n"), ToDeg(best_diff));
|
||||
|
||||
_gyro_offset = best_avg;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
AP_InertialSensor::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
|
||||
{
|
||||
_init_accel(delay_cb, flash_leds_cb);
|
||||
|
||||
// save calibration
|
||||
save_parameters();
|
||||
}
|
||||
|
||||
void
|
||||
AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
|
||||
{
|
||||
int16_t flashcount = 0;
|
||||
Vector3f ins_accel;
|
||||
Vector3f prev;
|
||||
Vector3f accel_offset;
|
||||
float total_change;
|
||||
float max_offset;
|
||||
|
||||
// cold start
|
||||
delay_cb(500);
|
||||
|
||||
Serial.printf_P(PSTR("Init Accel"));
|
||||
|
||||
// clear accelerometer offsets and scaling
|
||||
_accel_offset = Vector3f(0,0,0);
|
||||
_accel_scale = Vector3f(1,1,1);
|
||||
|
||||
// initialise accel offsets to a large value the first time
|
||||
// this will force us to calibrate accels at least twice
|
||||
accel_offset = Vector3f(500, 500, 500);
|
||||
|
||||
// loop until we calculate acceptable offsets
|
||||
do {
|
||||
// get latest accelerometer values
|
||||
update();
|
||||
ins_accel = get_accel();
|
||||
|
||||
// store old offsets
|
||||
prev = accel_offset;
|
||||
|
||||
// get new offsets
|
||||
accel_offset = ins_accel;
|
||||
|
||||
// We take some readings...
|
||||
for(int16_t i = 0; i < 50; i++) {
|
||||
|
||||
delay_cb(20);
|
||||
update();
|
||||
ins_accel = get_accel();
|
||||
|
||||
// low pass filter the offsets
|
||||
accel_offset = accel_offset * 0.9 + ins_accel * 0.1;
|
||||
|
||||
// display some output to the user
|
||||
if(flashcount == 5) {
|
||||
Serial.printf_P(PSTR("*"));
|
||||
FLASH_LEDS(true);
|
||||
}
|
||||
|
||||
if(flashcount >= 10) {
|
||||
flashcount = 0;
|
||||
FLASH_LEDS(false);
|
||||
}
|
||||
flashcount++;
|
||||
}
|
||||
|
||||
// null gravity from the Z accel
|
||||
// TO-DO: replace with gravity #define form location.cpp
|
||||
accel_offset.z += GRAVITY;
|
||||
|
||||
total_change = fabs(prev.x - accel_offset.x) + fabs(prev.y - accel_offset.y) + fabs(prev.z - accel_offset.z);
|
||||
max_offset = (accel_offset.x > accel_offset.y) ? accel_offset.x : accel_offset.y;
|
||||
max_offset = (max_offset > accel_offset.z) ? max_offset : accel_offset.z;
|
||||
|
||||
delay_cb(500);
|
||||
} while ( total_change > AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE || max_offset > AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET);
|
||||
|
||||
// set the global accel offsets
|
||||
_accel_offset = accel_offset;
|
||||
|
||||
Serial.printf_P(PSTR(" "));
|
||||
|
||||
}
|
||||
|
||||
// perform accelerometer calibration including providing user instructions and feedback
|
||||
void AP_InertialSensor::calibrate_accel(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on))
|
||||
{
|
||||
Vector3f samples[6];
|
||||
Vector3f new_offsets;
|
||||
Vector3f new_scaling;
|
||||
|
||||
// clear accelerometer offsets and scaling
|
||||
_accel_offset = Vector3f(0,0,0);
|
||||
_accel_scale = Vector3f(1,1,1);
|
||||
|
||||
// capture data from 6 positions
|
||||
for(int16_t i=0; i<6; i++ ) {
|
||||
|
||||
// display message to user
|
||||
Serial.print_P(PSTR("Place APM "));
|
||||
switch( i ) {
|
||||
case 0:
|
||||
Serial.print_P(PSTR("level"));
|
||||
break;
|
||||
case 1:
|
||||
Serial.print_P(PSTR("on it's left side"));
|
||||
break;
|
||||
case 2:
|
||||
Serial.print_P(PSTR("on it's right side"));
|
||||
break;
|
||||
case 3:
|
||||
Serial.print_P(PSTR("nose down"));
|
||||
break;
|
||||
case 4:
|
||||
Serial.print_P(PSTR("nose up"));
|
||||
break;
|
||||
case 5:
|
||||
Serial.print_P(PSTR("on it's back"));
|
||||
break;
|
||||
default:
|
||||
// should never happen
|
||||
break;
|
||||
}
|
||||
Serial.print_P(PSTR(" and press any key.\n"));
|
||||
|
||||
// wait for user input
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
// clear unput buffer
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
|
||||
// clear out any existing samples from ins
|
||||
update();
|
||||
|
||||
// wait until we have 32 samples
|
||||
while( num_samples_available() < 32 * SAMPLE_UNIT ) {
|
||||
delay(1);
|
||||
}
|
||||
|
||||
// read samples from ins
|
||||
update();
|
||||
|
||||
// capture sample
|
||||
samples[i] = get_accel();
|
||||
|
||||
// display sample
|
||||
Serial.printf_P(PSTR("Acc X:%4.2f\tY:%4.2f\tZ:%4.2f\n"),samples[i].x,samples[i].y,samples[i].z);
|
||||
}
|
||||
|
||||
// run the calibration routine
|
||||
if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
|
||||
_accel_offset.set(new_offsets);
|
||||
_accel_scale.set(new_scaling);
|
||||
// save calibration
|
||||
save_parameters();
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Accel calibration failed"));
|
||||
}
|
||||
}
|
||||
|
||||
// _calibrate_model - perform low level accel calibration
|
||||
// accel_sample are accelerometer samples collected in 6 different positions
|
||||
// accel_offsets are output from the calibration routine
|
||||
// accel_scale are output from the calibration routine
|
||||
// returns true if successful
|
||||
bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale )
|
||||
{
|
||||
int16_t i;
|
||||
int16_t num_iterations = 0;
|
||||
float eps = 0.000000001;
|
||||
float change = 100.0;
|
||||
float data[3];
|
||||
float beta[6];
|
||||
float delta[6];
|
||||
float ds[6];
|
||||
float JS[6][6];
|
||||
|
||||
// reset
|
||||
beta[0] = beta[1] = beta[2] = 0;
|
||||
beta[3] = beta[4] = beta[5] = 1.0/GRAVITY;
|
||||
|
||||
while( num_iterations < 20 && change > eps ) {
|
||||
num_iterations++;
|
||||
|
||||
_calibrate_reset_matrices(ds, JS);
|
||||
|
||||
for( i=0; i<6; i++ ) {
|
||||
data[0] = accel_sample[i].x;
|
||||
data[1] = accel_sample[i].y;
|
||||
data[2] = accel_sample[i].z;
|
||||
_calibrate_update_matrices(ds, JS, beta, data);
|
||||
}
|
||||
|
||||
_calibrate_find_delta(ds, JS, delta);
|
||||
|
||||
change = delta[0]*delta[0] +
|
||||
delta[0]*delta[0] +
|
||||
delta[1]*delta[1] +
|
||||
delta[2]*delta[2] +
|
||||
delta[3]*delta[3] / (beta[3]*beta[3]) +
|
||||
delta[4]*delta[4] / (beta[4]*beta[4]) +
|
||||
delta[5]*delta[5] / (beta[5]*beta[5]);
|
||||
|
||||
for( i=0; i<6; i++ ) {
|
||||
beta[i] -= delta[i];
|
||||
}
|
||||
}
|
||||
|
||||
// copy results out
|
||||
accel_scale.x = beta[3] * GRAVITY;
|
||||
accel_scale.y = beta[4] * GRAVITY;
|
||||
accel_scale.z = beta[5] * GRAVITY;
|
||||
accel_offsets.x = beta[0] * accel_scale.x;
|
||||
accel_offsets.y = beta[1] * accel_scale.y;
|
||||
accel_offsets.z = beta[2] * accel_scale.z;
|
||||
|
||||
// always return success
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_InertialSensor::_calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3])
|
||||
{
|
||||
int16_t j, k;
|
||||
float dx, b;
|
||||
float residual = 1.0;
|
||||
float jacobian[6];
|
||||
|
||||
for( j=0; j<3; j++ ) {
|
||||
b = beta[3+j];
|
||||
dx = (float)data[j] - beta[j];
|
||||
residual -= b*b*dx*dx;
|
||||
jacobian[j] = 2.0*b*b*dx;
|
||||
jacobian[3+j] = -2.0*b*dx*dx;
|
||||
}
|
||||
|
||||
for( j=0; j<6; j++ ) {
|
||||
dS[j] += jacobian[j]*residual;
|
||||
for( k=0; k<6; k++ ) {
|
||||
JS[j][k] += jacobian[j]*jacobian[k];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// _calibrate_reset_matrices - clears matrices
|
||||
void AP_InertialSensor::_calibrate_reset_matrices(float dS[6], float JS[6][6])
|
||||
{
|
||||
int16_t j,k;
|
||||
for( j=0; j<6; j++ ) {
|
||||
dS[j] = 0.0;
|
||||
for( k=0; k<6; k++ ) {
|
||||
JS[j][k] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_InertialSensor::_calibrate_find_delta(float dS[6], float JS[6][6], float delta[6])
|
||||
{
|
||||
//Solve 6-d matrix equation JS*x = dS
|
||||
//first put in upper triangular form
|
||||
int16_t i,j,k;
|
||||
float mu;
|
||||
|
||||
//make upper triangular
|
||||
for( i=0; i<6; i++ ) {
|
||||
//eliminate all nonzero entries below JS[i][i]
|
||||
for( j=i+1; j<6; j++ ) {
|
||||
mu = JS[i][j]/JS[i][i];
|
||||
if( mu != 0.0 ) {
|
||||
dS[j] -= mu*dS[i];
|
||||
for( k=j; k<6; k++ ) {
|
||||
JS[k][j] -= mu*JS[k][i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//back-substitute
|
||||
for( i=5; i>=0; i-- ) {
|
||||
dS[i] /= JS[i][i];
|
||||
JS[i][i] = 1.0;
|
||||
|
||||
for( j=0; j<i; j++ ) {
|
||||
mu = JS[i][j];
|
||||
dS[j] -= mu*dS[i];
|
||||
JS[i][j] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
for( i=0; i<6; i++ ) {
|
||||
delta[i] = dS[i];
|
||||
}
|
||||
}
|
||||
@ -3,26 +3,96 @@
|
||||
#ifndef __AP_INERTIAL_SENSOR_H__
|
||||
#define __AP_INERTIAL_SENSOR_H__
|
||||
|
||||
#include "../AP_Math/AP_Math.h"
|
||||
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
|
||||
|
||||
#define GRAVITY 9.80665
|
||||
// Gyro and Accelerometer calibration criteria
|
||||
#define AP_INERTIAL_SENSOR_ACCEL_TOT_MAX_OFFSET_CHANGE 4.0
|
||||
#define AP_INERTIAL_SENSOR_ACCEL_MAX_OFFSET 250.0
|
||||
|
||||
/* AP_InertialSensor is an abstraction for gyro and accel measurements
|
||||
* which are correctly aligned to the body axes and scaled to SI units.
|
||||
*/
|
||||
class AP_InertialSensor
|
||||
{
|
||||
public:
|
||||
AP_InertialSensor() {
|
||||
}
|
||||
AP_InertialSensor();
|
||||
|
||||
virtual uint16_t init( AP_PeriodicProcess * scheduler ) = 0;
|
||||
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,
|
||||
void (*delay_cb)(unsigned long t),
|
||||
void (*flash_leds_cb)(bool on),
|
||||
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), void (*flash_leds_cb)(bool on));
|
||||
|
||||
// perform accelerometer calibration including providing user instructions and feedback
|
||||
virtual void calibrate_accel(void (*callback)(unsigned long t), void (*flash_leds_cb)(bool on));
|
||||
|
||||
/// 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), void (*flash_leds_cb)(bool on));
|
||||
|
||||
/// Fetch the current gyro values
|
||||
///
|
||||
/// @returns vector of rotational rates in radians/sec
|
||||
///
|
||||
Vector3f get_gyro(void) { return _gyro; }
|
||||
|
||||
// set gyro offsets in radians/sec
|
||||
Vector3f get_gyro_offsets(void) { return _gyro_offset; }
|
||||
void set_gyro_offsets(Vector3f offsets) { _gyro_offset.set(offsets); }
|
||||
|
||||
/// Fetch the current accelerometer values
|
||||
///
|
||||
/// @returns vector of current accelerations in m/s/s
|
||||
///
|
||||
Vector3f get_accel(void) { return _accel; }
|
||||
|
||||
// get accel offsets in m/s/s
|
||||
Vector3f get_accel_offsets() { return _accel_offset; }
|
||||
void set_accel_offsets(Vector3f offsets) { _accel_offset.set(offsets); }
|
||||
|
||||
// get accel scale
|
||||
Vector3f get_accel_scale() { return _accel_scale; }
|
||||
|
||||
// sensor specific init to be overwritten by descendant classes
|
||||
// To-Do: should be combined with init above?
|
||||
virtual uint16_t _init( AP_PeriodicProcess * scheduler ) = 0;
|
||||
|
||||
/* Update the sensor data, so that getters are nonblocking.
|
||||
* Returns a bool of whether data was updated or not.
|
||||
*/
|
||||
virtual bool update() = 0;
|
||||
virtual bool update() = 0;
|
||||
|
||||
// check if the sensors have new data
|
||||
virtual bool new_data_available(void) = 0;
|
||||
virtual bool new_data_available(void) = 0;
|
||||
|
||||
/* Getters for individual gyro axes.
|
||||
* Gyros have correct coordinate frame and units (degrees per second).
|
||||
@ -31,9 +101,6 @@ public:
|
||||
virtual float gy() = 0;
|
||||
virtual float gz() = 0;
|
||||
|
||||
/* Same data as above gyro getters, written to array as { gx, gy, gz } */
|
||||
virtual void get_gyros( float * ) = 0;
|
||||
|
||||
/* Getters for individual accel axes.
|
||||
* Accels have correct coordinate frame ( flat level ax, ay = 0; az = -9.81)
|
||||
* and units (meters per second squared).
|
||||
@ -42,29 +109,61 @@ public:
|
||||
virtual float ay() = 0;
|
||||
virtual float az() = 0;
|
||||
|
||||
/* Same data as above accel getters, written to array as { ax, ay, az } */
|
||||
virtual void get_accels( float * ) = 0;
|
||||
|
||||
/* Same data as above accel and gyro getters, written to array as
|
||||
* { gx, gy, gz, ax, ay, az }
|
||||
*/
|
||||
virtual void get_sensors( float * ) = 0;
|
||||
|
||||
/* Temperature, in degrees celsius, of the gyro. */
|
||||
virtual float temperature() = 0;
|
||||
|
||||
/* sample_time returns the time period in microseconds
|
||||
/* get_delta_time returns the time period in seconds
|
||||
* overwhich the sensor data was collected
|
||||
*/
|
||||
virtual uint32_t sample_time() = 0;
|
||||
virtual float get_delta_time() { return (float)get_delta_time_micros() * 1.0e-6; }
|
||||
virtual uint32_t get_delta_time_micros() = 0;
|
||||
|
||||
// get_last_sample_time_micros returns the time in microseconds that the last sample was taken
|
||||
//virtual uint32_t get_last_sample_time_micros() = 0;
|
||||
|
||||
// return the maximum gyro drift rate in radians/s/s. This
|
||||
// depends on what gyro chips are being used
|
||||
virtual float get_gyro_drift_rate(void) = 0;
|
||||
virtual float get_gyro_drift_rate(void) = 0;
|
||||
|
||||
// get number of samples read from the sensors
|
||||
virtual uint16_t num_samples_available() = 0;
|
||||
virtual uint16_t num_samples_available() = 0;
|
||||
|
||||
// class level parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
// no-save implementations of accel and gyro initialisation routines
|
||||
virtual void _init_accel(void (*delay_cb)(unsigned long t),
|
||||
void (*flash_leds_cb)(bool on) = NULL);
|
||||
virtual void _init_gyro(void (*delay_cb)(unsigned long t),
|
||||
void (*flash_leds_cb)(bool on) = NULL);
|
||||
|
||||
// _calibrate_accel - perform low level accel calibration
|
||||
virtual bool _calibrate_accel(Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale );
|
||||
virtual void _calibrate_update_matrices(float dS[6], float JS[6][6], float beta[6], float data[3]);
|
||||
virtual void _calibrate_reset_matrices(float dS[6], float JS[6][6]);
|
||||
virtual void _calibrate_find_delta(float dS[6], float JS[6][6], float delta[6]);
|
||||
|
||||
// load parameters from eeprom
|
||||
void load_parameters();
|
||||
|
||||
// save parameters to eeprom
|
||||
void save_parameters();
|
||||
|
||||
// Most recent accelerometer reading obtained by ::update
|
||||
Vector3f _accel;
|
||||
|
||||
// Most recent gyro reading obtained by ::update
|
||||
Vector3f _gyro;
|
||||
|
||||
// product id
|
||||
AP_Int16 _product_id;
|
||||
|
||||
// accelerometer scaling and offsets
|
||||
AP_Vector3f _accel_scale;
|
||||
AP_Vector3f _accel_offset;
|
||||
AP_Vector3f _gyro_offset;
|
||||
};
|
||||
|
||||
#include "AP_InertialSensor_Oilpan.h"
|
||||
|
||||
@ -10,6 +10,9 @@
|
||||
#include <wiring.h>
|
||||
#endif
|
||||
|
||||
// MPU6000 accelerometer scaling
|
||||
#define MPU6000_ACCEL_SCALE_1G (GRAVITY / 4096.0)
|
||||
|
||||
// MPU 6000 registers
|
||||
#define MPUREG_XG_OFFS_TC 0x00
|
||||
#define MPUREG_YG_OFFS_TC 0x01
|
||||
@ -167,15 +170,6 @@
|
||||
*/
|
||||
const float AP_InertialSensor_MPU6000::_gyro_scale = (0.0174532 / 16.4);
|
||||
|
||||
/*
|
||||
* RS-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
|
||||
* accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
|
||||
*
|
||||
* See note below about accel scaling of engineering sample MPU6k
|
||||
* variants however
|
||||
*/
|
||||
const float AP_InertialSensor_MPU6000::_accel_scale = 9.81 / 4096.0;
|
||||
|
||||
/* pch: I believe the accel and gyro indicies are correct
|
||||
* but somone else should please confirm.
|
||||
*/
|
||||
@ -187,13 +181,13 @@ const int8_t AP_InertialSensor_MPU6000::_accel_data_sign[3] = { 1, 1, -1 };
|
||||
|
||||
const uint8_t AP_InertialSensor_MPU6000::_temp_data_index = 3;
|
||||
|
||||
int16_t AP_InertialSensor_MPU6000::_mpu6000_product_id = AP_PRODUCT_ID_NONE;
|
||||
|
||||
// variables to calculate time period over which a group of samples were collected
|
||||
static volatile uint32_t _delta_time_micros = 1; // time period overwhich samples were collected (initialise to non-zero number but will be overwritten on 2nd read in any case)
|
||||
static volatile uint32_t _delta_time_start_micros = 0; // time we start collecting sample (reset on update)
|
||||
static volatile uint32_t _last_sample_time_micros = 0; // time latest sample was collected
|
||||
|
||||
static uint8_t _product_id;
|
||||
|
||||
// DMP related static variables
|
||||
bool AP_InertialSensor_MPU6000::_dmp_initialised = false;
|
||||
uint8_t AP_InertialSensor_MPU6000::_fifoCountH; // high byte of number of elements in fifo buffer
|
||||
@ -201,28 +195,30 @@ uint8_t AP_InertialSensor_MPU6000::_fifoCountL; // low byte of n
|
||||
Quaternion AP_InertialSensor_MPU6000::quaternion; // holds the 4 quaternions representing attitude taken directly from the DMP
|
||||
AP_PeriodicProcess* AP_InertialSensor_MPU6000::_scheduler = NULL;
|
||||
|
||||
/*
|
||||
* RS-MPU-6000A-00.pdf, page 31, section 4.23 lists LSB sensitivity of
|
||||
* accel as 4096 LSB/mg at scale factor of +/- 8g (AFS_SEL==2)
|
||||
*
|
||||
* See note below about accel scaling of engineering sample MPU6k
|
||||
* variants however
|
||||
*/
|
||||
|
||||
AP_InertialSensor_MPU6000::AP_InertialSensor_MPU6000()
|
||||
{
|
||||
_gyro.x = 0;
|
||||
_gyro.y = 0;
|
||||
_gyro.z = 0;
|
||||
_accel.x = 0;
|
||||
_accel.y = 0;
|
||||
_accel.z = 0;
|
||||
_temp = 0;
|
||||
_initialised = false;
|
||||
_dmp_initialised = false;
|
||||
}
|
||||
|
||||
uint16_t AP_InertialSensor_MPU6000::init( AP_PeriodicProcess * scheduler )
|
||||
uint16_t AP_InertialSensor_MPU6000::_init( AP_PeriodicProcess * scheduler )
|
||||
{
|
||||
if (_initialised) return _product_id;
|
||||
if (_initialised) return _mpu6000_product_id;
|
||||
_initialised = true;
|
||||
_scheduler = scheduler; // store pointer to scheduler so that we can suspend/resume scheduler when we pull data from the MPU6000
|
||||
scheduler->suspend_timer();
|
||||
hardware_init();
|
||||
scheduler->resume_timer();
|
||||
return _product_id;
|
||||
return _mpu6000_product_id;
|
||||
}
|
||||
|
||||
// accumulation in ISR - must be read with interrupts disabled
|
||||
@ -232,7 +228,6 @@ static volatile int32_t _sum[7];
|
||||
// how many values we've accumulated since last read
|
||||
static volatile uint16_t _count;
|
||||
|
||||
|
||||
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
|
||||
|
||||
bool AP_InertialSensor_MPU6000::update( void )
|
||||
@ -240,6 +235,9 @@ bool AP_InertialSensor_MPU6000::update( void )
|
||||
int32_t sum[7];
|
||||
uint16_t count;
|
||||
float count_scale;
|
||||
Vector3f gyro_offset = _gyro_offset.get();
|
||||
Vector3f accel_scale = _accel_scale.get();
|
||||
Vector3f accel_offset = _accel_offset.get();
|
||||
|
||||
// wait for at least 1 sample
|
||||
while (_count == 0) /* nop */;
|
||||
@ -260,13 +258,13 @@ bool AP_InertialSensor_MPU6000::update( void )
|
||||
|
||||
count_scale = 1.0 / count;
|
||||
|
||||
_gyro.x = _gyro_scale * _gyro_data_sign[0] * sum[_gyro_data_index[0]] * count_scale;
|
||||
_gyro.y = _gyro_scale * _gyro_data_sign[1] * sum[_gyro_data_index[1]] * count_scale;
|
||||
_gyro.z = _gyro_scale * _gyro_data_sign[2] * sum[_gyro_data_index[2]] * count_scale;
|
||||
_gyro.x = _gyro_scale * _gyro_data_sign[0] * sum[_gyro_data_index[0]] * count_scale - gyro_offset.x;
|
||||
_gyro.y = _gyro_scale * _gyro_data_sign[1] * sum[_gyro_data_index[1]] * count_scale - gyro_offset.y;
|
||||
_gyro.z = _gyro_scale * _gyro_data_sign[2] * sum[_gyro_data_index[2]] * count_scale - gyro_offset.z;
|
||||
|
||||
_accel.x = _accel_scale * _accel_data_sign[0] * sum[_accel_data_index[0]] * count_scale;
|
||||
_accel.y = _accel_scale * _accel_data_sign[1] * sum[_accel_data_index[1]] * count_scale;
|
||||
_accel.z = _accel_scale * _accel_data_sign[2] * sum[_accel_data_index[2]] * count_scale;
|
||||
_accel.x = accel_scale.x * _accel_data_sign[0] * sum[_accel_data_index[0]] * count_scale * MPU6000_ACCEL_SCALE_1G - accel_offset.x;
|
||||
_accel.y = accel_scale.y * _accel_data_sign[1] * sum[_accel_data_index[1]] * count_scale * MPU6000_ACCEL_SCALE_1G - accel_offset.y;
|
||||
_accel.z = accel_scale.z * _accel_data_sign[2] * sum[_accel_data_index[2]] * count_scale * MPU6000_ACCEL_SCALE_1G - accel_offset.z;
|
||||
|
||||
_temp = _temp_to_celsius(sum[_temp_data_index] * count_scale);
|
||||
|
||||
@ -288,13 +286,6 @@ float AP_InertialSensor_MPU6000::gz() {
|
||||
return _gyro.z;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::get_gyros( float * g )
|
||||
{
|
||||
g[0] = _gyro.x;
|
||||
g[1] = _gyro.y;
|
||||
g[2] = _gyro.z;
|
||||
}
|
||||
|
||||
float AP_InertialSensor_MPU6000::ax() {
|
||||
return _accel.x;
|
||||
}
|
||||
@ -305,32 +296,10 @@ float AP_InertialSensor_MPU6000::az() {
|
||||
return _accel.z;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::get_accels( float * a )
|
||||
{
|
||||
a[0] = _accel.x;
|
||||
a[1] = _accel.y;
|
||||
a[2] = _accel.z;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_MPU6000::get_sensors( float * sensors )
|
||||
{
|
||||
sensors[0] = _gyro.x;
|
||||
sensors[1] = _gyro.y;
|
||||
sensors[2] = _gyro.z;
|
||||
sensors[3] = _accel.x;
|
||||
sensors[4] = _accel.y;
|
||||
sensors[5] = _accel.z;
|
||||
}
|
||||
|
||||
float AP_InertialSensor_MPU6000::temperature() {
|
||||
return _temp;
|
||||
}
|
||||
|
||||
uint32_t AP_InertialSensor_MPU6000::sample_time()
|
||||
{
|
||||
return _delta_time_micros;
|
||||
}
|
||||
|
||||
/*================ HARDWARE FUNCTIONS ==================== */
|
||||
|
||||
static int16_t spi_transfer_16(void)
|
||||
@ -438,11 +407,11 @@ void AP_InertialSensor_MPU6000::hardware_init()
|
||||
register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
|
||||
delay(1);
|
||||
|
||||
_product_id = register_read(MPUREG_PRODUCT_ID); // read the product ID rev c has 1/2 the sensitivity of rev d
|
||||
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);
|
||||
_mpu6000_product_id = register_read(MPUREG_PRODUCT_ID); // read the product ID rev c has 1/2 the sensitivity of rev d
|
||||
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
|
||||
|
||||
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
|
||||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)) {
|
||||
if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
|
||||
(_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
|
||||
// Accel scale 8g (4096 LSB/g)
|
||||
// Rev C has different scaling than rev D
|
||||
register_write(MPUREG_ACCEL_CONFIG,1<<3);
|
||||
@ -480,25 +449,33 @@ uint16_t AP_InertialSensor_MPU6000::num_samples_available()
|
||||
return _count;
|
||||
}
|
||||
|
||||
// get time (in microseconds) that last sample was captured
|
||||
uint32_t AP_InertialSensor_MPU6000::last_sample_time()
|
||||
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
|
||||
uint32_t AP_InertialSensor_MPU6000::get_delta_time_micros()
|
||||
{
|
||||
return _last_sample_time_micros;
|
||||
return _delta_time_micros;
|
||||
}
|
||||
|
||||
// Update gyro offsets with new values. Offsets provided in as scaled deg/sec values
|
||||
void AP_InertialSensor_MPU6000::set_gyro_offsets_scaled(float offX, float offY, float offZ)
|
||||
void AP_InertialSensor_MPU6000::push_gyro_offsets_to_dmp()
|
||||
{
|
||||
int16_t offsetX = offX / _gyro_scale * _gyro_data_sign[0];
|
||||
int16_t offsetY = offY / _gyro_scale * _gyro_data_sign[1];
|
||||
int16_t offsetZ = offZ / _gyro_scale * _gyro_data_sign[2];
|
||||
Vector3f gyro_offsets = _gyro_offset.get();
|
||||
|
||||
set_gyro_offsets(offsetX, offsetY, offsetZ);
|
||||
int16_t offsetX = gyro_offsets.x / _gyro_scale * _gyro_data_sign[0];
|
||||
int16_t offsetY = gyro_offsets.y / _gyro_scale * _gyro_data_sign[1];
|
||||
int16_t offsetZ = gyro_offsets.z / _gyro_scale * _gyro_data_sign[2];
|
||||
|
||||
set_dmp_gyro_offsets(offsetX, offsetY, offsetZ);
|
||||
|
||||
// remove ins level offsets to avoid double counting
|
||||
gyro_offsets.x = 0;
|
||||
gyro_offsets.y = 0;
|
||||
gyro_offsets.z = 0;
|
||||
_gyro_offset = gyro_offsets;
|
||||
}
|
||||
|
||||
// Update gyro offsets with new values. New offset values are substracted to actual offset values.
|
||||
// offset values in gyro LSB units (as read from registers)
|
||||
void AP_InertialSensor_MPU6000::set_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ)
|
||||
void AP_InertialSensor_MPU6000::set_dmp_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ)
|
||||
{
|
||||
int16_t aux_int;
|
||||
|
||||
@ -526,21 +503,24 @@ void AP_InertialSensor_MPU6000::set_gyro_offsets(int16_t offsetX, int16_t offset
|
||||
}
|
||||
}
|
||||
|
||||
// Update accel offsets with new values. Offsets provided in as scaled values (Gs?)
|
||||
void AP_InertialSensor_MPU6000::set_accel_offsets_scaled(float offX, float offY, float offZ)
|
||||
// Update accel offsets with new values. Offsets provided in as scaled values (1G)
|
||||
void AP_InertialSensor_MPU6000::push_accel_offsets_to_dmp()
|
||||
{
|
||||
int16_t offsetX = offX / _accel_scale * _accel_data_sign[0];
|
||||
int16_t offsetY = offY / _accel_scale * _accel_data_sign[1];
|
||||
int16_t offsetZ = offZ / _accel_scale * _accel_data_sign[2];
|
||||
Vector3f accel_offset = _accel_offset.get();
|
||||
Vector3f accel_scale = _accel_scale.get();
|
||||
int16_t offsetX = accel_offset.x / (accel_scale.x * _accel_data_sign[0] * MPU6000_ACCEL_SCALE_1G);
|
||||
int16_t offsetY = accel_offset.y / (accel_scale.y * _accel_data_sign[1] * MPU6000_ACCEL_SCALE_1G);
|
||||
int16_t offsetZ = accel_offset.z / (accel_scale.z * _accel_data_sign[2] * MPU6000_ACCEL_SCALE_1G);
|
||||
|
||||
set_accel_offsets(offsetX, offsetY, offsetZ);
|
||||
// strangely x and y are reversed
|
||||
set_dmp_accel_offsets(offsetY, offsetX, offsetZ);
|
||||
}
|
||||
|
||||
// set_accel_offsets - adds an offset to acceleromter readings
|
||||
// This is useful for dynamic acceleration correction (for example centripetal force correction)
|
||||
// and for the initial offset calibration
|
||||
// Input, accel offsets for X,Y and Z in LSB units (as read from raw values)
|
||||
void AP_InertialSensor_MPU6000::set_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ)
|
||||
void AP_InertialSensor_MPU6000::set_dmp_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ)
|
||||
{
|
||||
int aux_int;
|
||||
uint8_t regs[2];
|
||||
|
||||
@ -22,7 +22,7 @@ public:
|
||||
|
||||
AP_InertialSensor_MPU6000();
|
||||
|
||||
uint16_t init( AP_PeriodicProcess * scheduler );
|
||||
uint16_t _init( AP_PeriodicProcess * scheduler );
|
||||
static void dmp_init(); // Initialise MPU6000's DMP
|
||||
static void dmp_reset(); // Reset the DMP (required for changes in gains or offsets to take effect)
|
||||
|
||||
@ -32,29 +32,25 @@ public:
|
||||
float gx();
|
||||
float gy();
|
||||
float gz();
|
||||
void get_gyros( float * );
|
||||
float ax();
|
||||
float ay();
|
||||
float az();
|
||||
void get_accels( float * );
|
||||
void get_sensors( float * );
|
||||
float temperature();
|
||||
uint32_t sample_time();
|
||||
float get_gyro_drift_rate();
|
||||
|
||||
// set_gyro_offsets - updates gyro offsets in mpu6000 registers
|
||||
static void set_gyro_offsets_scaled(float offX, float offY, float offZ);
|
||||
static void set_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
|
||||
// push_gyro_offsets_to_dmp - updates gyro offsets in mpu6000 registers
|
||||
void push_gyro_offsets_to_dmp();
|
||||
void set_dmp_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
|
||||
|
||||
// set_accel_offsets - updates accel offsets in DMP registers
|
||||
static void set_accel_offsets_scaled(float offX, float offY, float offZ);
|
||||
static void set_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
|
||||
// push_accel_offsets_to_dmp - updates accel offsets in DMP registers
|
||||
void push_accel_offsets_to_dmp();
|
||||
void set_dmp_accel_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ);
|
||||
|
||||
// get number of samples read from the sensors
|
||||
uint16_t num_samples_available();
|
||||
// num_samples_available - get number of samples read from the sensors
|
||||
uint16_t num_samples_available();
|
||||
|
||||
// get time (in microseconds) that last sample was captured
|
||||
uint32_t last_sample_time();
|
||||
// get_delta_time returns the time period in seconds overwhich the sensor data was collected
|
||||
uint32_t get_delta_time_micros();
|
||||
|
||||
private:
|
||||
|
||||
@ -64,13 +60,10 @@ private:
|
||||
static void register_write( uint8_t reg, uint8_t val );
|
||||
static void hardware_init();
|
||||
|
||||
Vector3f _gyro;
|
||||
Vector3f _accel;
|
||||
float _temp;
|
||||
|
||||
float _temp_to_celsius( uint16_t );
|
||||
|
||||
static const float _accel_scale;
|
||||
static const float _gyro_scale;
|
||||
|
||||
static const uint8_t _gyro_data_index[3];
|
||||
@ -85,6 +78,7 @@ private:
|
||||
|
||||
// ensure we can't initialise twice
|
||||
bool _initialised;
|
||||
static int16_t _mpu6000_product_id;
|
||||
|
||||
// dmp related methods and parameters
|
||||
static void dmp_register_write(uint8_t bank, uint8_t address, uint8_t num_bytes, uint8_t data[]); // Method to write multiple bytes into dmp registers. Requires a "bank"
|
||||
|
||||
@ -20,14 +20,11 @@ const float AP_InertialSensor_Oilpan::_adc_constraint = 900;
|
||||
// ADXL335 Sensitivity(from datasheet) => 330mV/g,
|
||||
// 0.8mV/ADC step => 330/0.8 = 412
|
||||
// Tested value : 418
|
||||
// 1G in the raw data coming from the accelerometer
|
||||
// Value based on actual sample data from 20 boards
|
||||
const float AP_InertialSensor_Oilpan::_gravity = 423.8;
|
||||
|
||||
///< would like to use _gravity here, but cannot
|
||||
//const float AP_InertialSensor_Oilpan::_accel_x_scale = 9.80665 / 413.195;
|
||||
//const float AP_InertialSensor_Oilpan::_accel_y_scale = 9.80665 / 412.985;
|
||||
//const float AP_InertialSensor_Oilpan::_accel_z_scale = 9.80665 / 403.69;
|
||||
// Oilpan accelerometer scaling & offsets
|
||||
#define OILPAN_ACCEL_SCALE_1G (GRAVITY * 2.0 / (2465.0 - 1617.0))
|
||||
#define OILPAN_RAW_ACCEL_OFFSET ((2465.0 - 1617.0) / 2)
|
||||
#define OILPAN_RAW_GYRO_OFFSET 1658.0
|
||||
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
// IDG500 Sensitivity (from datasheet) => 2.0mV/degree/s,
|
||||
@ -37,42 +34,17 @@ const float AP_InertialSensor_Oilpan::_gyro_gain_x = ToRad(0.4);
|
||||
const float AP_InertialSensor_Oilpan::_gyro_gain_y = ToRad(0.41);
|
||||
const float AP_InertialSensor_Oilpan::_gyro_gain_z = ToRad(0.41);
|
||||
|
||||
const AP_Param::GroupInfo AP_InertialSensor_Oilpan::var_info[] PROGMEM = {
|
||||
// index 0 was used for the old orientation matrix
|
||||
AP_GROUPINFO("XH", 0, AP_InertialSensor_Oilpan, _x_high, 2465),
|
||||
AP_GROUPINFO("XL", 1, AP_InertialSensor_Oilpan, _x_low, 1617),
|
||||
AP_GROUPINFO("YH", 2, AP_InertialSensor_Oilpan, _y_high, 2465),
|
||||
AP_GROUPINFO("YL", 3, AP_InertialSensor_Oilpan, _y_low, 1617),
|
||||
AP_GROUPINFO("ZH", 4, AP_InertialSensor_Oilpan, _z_high, 2465),
|
||||
AP_GROUPINFO("ZL", 5, AP_InertialSensor_Oilpan, _z_low, 1617),
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
||||
/* ------ Public functions -------------------------------------------*/
|
||||
|
||||
AP_InertialSensor_Oilpan::AP_InertialSensor_Oilpan( AP_ADC * adc ) :
|
||||
_adc(adc)
|
||||
{
|
||||
_gyro.x = 0;
|
||||
_gyro.y = 0;
|
||||
_gyro.z = 0;
|
||||
_accel.x = 0;
|
||||
_accel.y = 0;
|
||||
_accel.z = 0;
|
||||
}
|
||||
|
||||
uint16_t AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler)
|
||||
uint16_t AP_InertialSensor_Oilpan::_init( AP_PeriodicProcess * scheduler)
|
||||
{
|
||||
_adc->Init(scheduler);
|
||||
|
||||
_accel_mid.x = (_x_high + _x_low) / 2;
|
||||
_accel_mid.y = (_y_high + _y_low) / 2;
|
||||
_accel_mid.z = (_z_high + _z_low) / 2;
|
||||
_accel_scale.x = 9.80665 / ((float)_x_high - _accel_mid.x);
|
||||
_accel_scale.y = 9.80665 / ((float)_y_high - _accel_mid.y);
|
||||
_accel_scale.z = 9.80665 / ((float)_z_high - _accel_mid.z);
|
||||
|
||||
#if defined(DESKTOP_BUILD)
|
||||
return AP_PRODUCT_ID_SITL;
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
@ -85,22 +57,20 @@ uint16_t AP_InertialSensor_Oilpan::init( AP_PeriodicProcess * scheduler)
|
||||
bool AP_InertialSensor_Oilpan::update()
|
||||
{
|
||||
float adc_values[6];
|
||||
Vector3f gyro_offset = _gyro_offset.get();
|
||||
Vector3f accel_scale = _accel_scale.get();
|
||||
Vector3f accel_offset = _accel_offset.get();
|
||||
|
||||
_sample_time = _adc->Ch6(_sensors, adc_values);
|
||||
_delta_time_micros = _adc->Ch6(_sensors, adc_values);
|
||||
_temp = _adc->Ch(_gyro_temp_ch);
|
||||
|
||||
_gyro.x = _gyro_gain_x * _sensor_signs[0] * _gyro_apply_std_offset( adc_values[0] );
|
||||
_gyro.y = _gyro_gain_y * _sensor_signs[1] * _gyro_apply_std_offset( adc_values[1] );
|
||||
_gyro.z = _gyro_gain_z * _sensor_signs[2] * _gyro_apply_std_offset( adc_values[2] );
|
||||
|
||||
// _accel.x = _accel_x_scale * _sensor_signs[3] * _accel_apply_std_offset( adc_values[3] );
|
||||
// _accel.y = _accel_y_scale * _sensor_signs[4] * _accel_apply_std_offset( adc_values[4] );
|
||||
// _accel.z = _accel_z_scale * _sensor_signs[5] * _accel_apply_std_offset( adc_values[5] );
|
||||
|
||||
_accel.x = _accel_scale.x * _sensor_signs[3] * (adc_values[3] - _accel_mid.x);
|
||||
_accel.y = _accel_scale.y * _sensor_signs[4] * (adc_values[4] - _accel_mid.y);
|
||||
_accel.z = _accel_scale.z * _sensor_signs[5] * (adc_values[5] - _accel_mid.z);
|
||||
_gyro.x = _gyro_gain_x * _sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) - gyro_offset.x;
|
||||
_gyro.y = _gyro_gain_y * _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) - gyro_offset.y;
|
||||
_gyro.z = _gyro_gain_z * _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) - gyro_offset.z;
|
||||
|
||||
_accel.x = accel_scale.x * _sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET) * OILPAN_ACCEL_SCALE_1G - accel_offset.x;
|
||||
_accel.y = accel_scale.y * _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET) * OILPAN_ACCEL_SCALE_1G - accel_offset.y;
|
||||
_accel.z = accel_scale.z * _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET) * OILPAN_ACCEL_SCALE_1G - accel_offset.z;
|
||||
|
||||
/*
|
||||
* X = 1619.30 to 2445.69
|
||||
@ -126,13 +96,6 @@ float AP_InertialSensor_Oilpan::gz() {
|
||||
return _gyro.z;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Oilpan::get_gyros( float * g )
|
||||
{
|
||||
g[0] = _gyro.x;
|
||||
g[1] = _gyro.y;
|
||||
g[2] = _gyro.z;
|
||||
}
|
||||
|
||||
float AP_InertialSensor_Oilpan::ax() {
|
||||
return _accel.x;
|
||||
}
|
||||
@ -143,45 +106,17 @@ float AP_InertialSensor_Oilpan::az() {
|
||||
return _accel.z;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Oilpan::get_accels( float * a )
|
||||
{
|
||||
a[0] = _accel.x;
|
||||
a[1] = _accel.y;
|
||||
a[2] = _accel.z;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Oilpan::get_sensors( float * sensors )
|
||||
{
|
||||
sensors[0] = _gyro.x;
|
||||
sensors[1] = _gyro.y;
|
||||
sensors[2] = _gyro.z;
|
||||
sensors[3] = _accel.x;
|
||||
sensors[4] = _accel.y;
|
||||
sensors[5] = _accel.z;
|
||||
}
|
||||
|
||||
float AP_InertialSensor_Oilpan::temperature() {
|
||||
return _temp;
|
||||
}
|
||||
|
||||
uint32_t AP_InertialSensor_Oilpan::sample_time() {
|
||||
return _sample_time;
|
||||
uint32_t AP_InertialSensor_Oilpan::get_delta_time_micros() {
|
||||
return _delta_time_micros;
|
||||
}
|
||||
|
||||
/* ------ Private functions -------------------------------------------*/
|
||||
|
||||
float AP_InertialSensor_Oilpan::_gyro_apply_std_offset( float adc_value )
|
||||
{
|
||||
/* Magic number from AP_ADC_Oilpan.h */
|
||||
return ((float) adc_value ) - 1658.0f;
|
||||
}
|
||||
|
||||
float AP_InertialSensor_Oilpan::_accel_apply_std_offset( float adc_value )
|
||||
{
|
||||
/* Magic number from AP_ADC_Oilpan.h */
|
||||
return ((float) adc_value ) - 2041.0f;
|
||||
}
|
||||
|
||||
// return the oilpan gyro drift rate in radian/s/s
|
||||
float AP_InertialSensor_Oilpan::get_gyro_drift_rate(void)
|
||||
{
|
||||
|
||||
@ -17,49 +17,30 @@ public:
|
||||
AP_InertialSensor_Oilpan( AP_ADC * adc );
|
||||
|
||||
/* Concrete implementation of AP_InertialSensor functions: */
|
||||
uint16_t init(AP_PeriodicProcess * scheduler);
|
||||
uint16_t _init(AP_PeriodicProcess * scheduler);
|
||||
bool update();
|
||||
bool new_data_available();
|
||||
float gx();
|
||||
float gy();
|
||||
float gz();
|
||||
void get_gyros( float * );
|
||||
float ax();
|
||||
float ay();
|
||||
float az();
|
||||
void get_accels( float * );
|
||||
void get_sensors( float * );
|
||||
float temperature();
|
||||
uint32_t sample_time();
|
||||
uint32_t get_delta_time_micros(); // get_delta_time returns the time period in seconds overwhich the sensor data was collected
|
||||
//uint32_t get_last_sample_time_micros(); // last_sample_time - get time (in microseconds) that last sample was captured
|
||||
float get_gyro_drift_rate();
|
||||
|
||||
// get number of samples read from the sensors
|
||||
uint16_t num_samples_available();
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
AP_Int16 _x_high;
|
||||
AP_Int16 _x_low;
|
||||
AP_Int16 _y_high;
|
||||
AP_Int16 _y_low;
|
||||
AP_Int16 _z_high;
|
||||
AP_Int16 _z_low;
|
||||
|
||||
Vector3f _accel_scale;
|
||||
|
||||
private:
|
||||
Vector3f _gyro;
|
||||
Vector3f _accel;
|
||||
|
||||
Vector3f _accel_high;
|
||||
Vector3f _accel_low;
|
||||
Vector3f _accel_mid;
|
||||
|
||||
AP_ADC * _adc;
|
||||
|
||||
float _temp;
|
||||
|
||||
uint32_t _sample_time;
|
||||
uint32_t _delta_time_micros;
|
||||
|
||||
static const uint8_t _sensors[6];
|
||||
static const int8_t _sensor_signs[6];
|
||||
@ -72,9 +53,6 @@ private:
|
||||
static const float _gyro_gain_z;
|
||||
|
||||
static const float _adc_constraint;
|
||||
|
||||
float _gyro_apply_std_offset( float adc_value );
|
||||
float _accel_apply_std_offset( float adc_value );
|
||||
};
|
||||
|
||||
#endif // __AP_INERTIAL_SENSOR_OILPAN_H__
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
|
||||
#include "AP_InertialSensor_Stub.h"
|
||||
|
||||
uint16_t AP_InertialSensor_Stub::init( AP_PeriodicProcess * scheduler ) {
|
||||
uint16_t AP_InertialSensor_Stub::_init( AP_PeriodicProcess * scheduler ) {
|
||||
return AP_PRODUCT_ID_NONE;
|
||||
}
|
||||
|
||||
@ -26,9 +26,6 @@ float AP_InertialSensor_Stub::gz() {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Stub::get_gyros( float * g ) {
|
||||
}
|
||||
|
||||
float AP_InertialSensor_Stub::ax() {
|
||||
return 0.0f;
|
||||
}
|
||||
@ -39,18 +36,14 @@ float AP_InertialSensor_Stub::az() {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Stub::get_accels( float * a ) {
|
||||
}
|
||||
void AP_InertialSensor_Stub::get_sensors( float * sensors ) {
|
||||
}
|
||||
|
||||
float AP_InertialSensor_Stub::temperature() {
|
||||
return 0.0;
|
||||
}
|
||||
uint32_t AP_InertialSensor_Stub::sample_time() {
|
||||
uint32_t AP_InertialSensor_Stub::get_delta_time_micros() {
|
||||
return 0;
|
||||
}
|
||||
void AP_InertialSensor_Stub::reset_sample_time() {
|
||||
uint32_t AP_InertialSensor_Stub::get_last_sample_time_micros() {
|
||||
return 0;
|
||||
}
|
||||
float AP_InertialSensor_Stub::get_gyro_drift_rate(void) {
|
||||
return 0.0;
|
||||
|
||||
@ -16,7 +16,7 @@ public:
|
||||
AP_InertialSensor_Stub() {
|
||||
}
|
||||
|
||||
uint16_t init( AP_PeriodicProcess * scheduler );
|
||||
uint16_t _init( AP_PeriodicProcess * scheduler );
|
||||
|
||||
/* Concrete implementation of AP_InertialSensor functions: */
|
||||
bool update();
|
||||
@ -24,15 +24,12 @@ public:
|
||||
float gx();
|
||||
float gy();
|
||||
float gz();
|
||||
void get_gyros( float * );
|
||||
float ax();
|
||||
float ay();
|
||||
float az();
|
||||
void get_accels( float * );
|
||||
void get_sensors( float * );
|
||||
float temperature();
|
||||
uint32_t sample_time();
|
||||
void reset_sample_time();
|
||||
uint32_t get_delta_time_micros();
|
||||
uint32_t get_last_sample_time_micros();
|
||||
float get_gyro_drift_rate();
|
||||
uint16_t num_samples_available();
|
||||
};
|
||||
|
||||
@ -8,15 +8,34 @@
|
||||
#include <SPI.h>
|
||||
#include <Arduino_Mega_ISR_Registry.h>
|
||||
#include <AP_PeriodicProcess.h>
|
||||
#include <AP_ADC.h>
|
||||
#include <AP_InertialSensor.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_Common.h>
|
||||
|
||||
#define APM_HARDWARE_APM1 1
|
||||
#define APM_HARDWARE_APM2 2
|
||||
|
||||
#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2
|
||||
//#define CONFIG_APM_HARDWARE APM_HARDWARE_APM1
|
||||
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
#define SAMPLE_UNIT 1
|
||||
#else
|
||||
#define SAMPLE_UNIT 5 // we need 5x as many samples on the oilpan
|
||||
#endif
|
||||
|
||||
FastSerialPort(Serial, 0);
|
||||
|
||||
Arduino_Mega_ISR_Registry isr_registry;
|
||||
AP_TimerProcess scheduler;
|
||||
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
|
||||
AP_InertialSensor_MPU6000 ins;
|
||||
#else
|
||||
AP_ADC_ADS7844 adc;
|
||||
AP_InertialSensor_Oilpan ins(&adc);
|
||||
#endif
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
@ -33,21 +52,149 @@ void setup(void)
|
||||
pinMode(40, OUTPUT);
|
||||
digitalWrite(40, HIGH);
|
||||
|
||||
ins.init(&scheduler);
|
||||
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM1
|
||||
adc.Init(&scheduler); // APM ADC library initialization
|
||||
#endif
|
||||
ins.init(AP_InertialSensor::COLD_START, delay, NULL, &scheduler);
|
||||
|
||||
// display initial values
|
||||
display_offsets_and_scaling();
|
||||
}
|
||||
|
||||
void loop(void)
|
||||
{
|
||||
float accel[3];
|
||||
float gyro[3];
|
||||
float temperature;
|
||||
int16_t user_input;
|
||||
|
||||
delay(20);
|
||||
ins.update();
|
||||
ins.get_gyros(gyro);
|
||||
ins.get_accels(accel);
|
||||
temperature = ins.temperature();
|
||||
Serial.println();
|
||||
Serial.println("Menu: ");
|
||||
Serial.println(" c) calibrate accelerometers");
|
||||
Serial.println(" d) display offsets and scaling");
|
||||
Serial.println(" l) level (capture offsets from level)");
|
||||
Serial.println(" t) test");
|
||||
|
||||
Serial.printf("AX: %f AY: %f AZ: %f GX: %f GY: %f GZ: %f T=%f\n",
|
||||
accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], temperature);
|
||||
// wait for user input
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
|
||||
// read in user input
|
||||
while( Serial.available() ) {
|
||||
user_input = Serial.read();
|
||||
|
||||
if( user_input == 'c' || user_input == 'C' ) {
|
||||
run_calibration();
|
||||
display_offsets_and_scaling();
|
||||
}
|
||||
|
||||
if( user_input == 'd' || user_input == 'D' ) {
|
||||
display_offsets_and_scaling();
|
||||
}
|
||||
|
||||
if( user_input == 'l' || user_input == 'L' ) {
|
||||
run_level();
|
||||
display_offsets_and_scaling();
|
||||
}
|
||||
|
||||
if( user_input == 't' || user_input == 'T' ) {
|
||||
run_test();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void run_calibration()
|
||||
{
|
||||
// clear off any other characters (like line feeds,etc)
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
|
||||
ins.calibrate_accel(delay, NULL);
|
||||
}
|
||||
|
||||
void display_offsets_and_scaling()
|
||||
{
|
||||
Vector3f accel_offsets = ins.get_accel_offsets();
|
||||
Vector3f accel_scale = ins.get_accel_scale();
|
||||
Vector3f gyro_offsets = ins.get_gyro_offsets();
|
||||
|
||||
// display results
|
||||
Serial.printf_P(PSTR("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
|
||||
accel_offsets.x,
|
||||
accel_offsets.y,
|
||||
accel_offsets.z);
|
||||
Serial.printf_P(PSTR("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
|
||||
accel_scale.x,
|
||||
accel_scale.y,
|
||||
accel_scale.z);
|
||||
Serial.printf_P(PSTR("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n"),
|
||||
gyro_offsets.x,
|
||||
gyro_offsets.y,
|
||||
gyro_offsets.z);
|
||||
}
|
||||
|
||||
void run_level()
|
||||
{
|
||||
// clear off any input in the buffer
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
|
||||
// display message to user
|
||||
Serial.print("Place APM on a level surface and press any key..\n");
|
||||
|
||||
// wait for user input
|
||||
while( !Serial.available() ) {
|
||||
delay(20);
|
||||
}
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
|
||||
// run accel level
|
||||
ins.init_accel(delay, NULL);
|
||||
|
||||
// display results
|
||||
display_offsets_and_scaling();
|
||||
}
|
||||
|
||||
void run_test()
|
||||
{
|
||||
Vector3f accel;
|
||||
Vector3f gyro;
|
||||
float temperature;
|
||||
float length;
|
||||
|
||||
// flush any user input
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
|
||||
// clear out any existing samples from ins
|
||||
ins.update();
|
||||
|
||||
// loop as long as user does not press a key
|
||||
while( !Serial.available() ) {
|
||||
|
||||
// wait until we have 8 samples
|
||||
while( ins.num_samples_available() < 8 * SAMPLE_UNIT ) {
|
||||
delay(1);
|
||||
}
|
||||
|
||||
// read samples from ins
|
||||
ins.update();
|
||||
accel = ins.get_accel();
|
||||
gyro = ins.get_gyro();
|
||||
temperature = ins.temperature();
|
||||
|
||||
length = sqrt(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
|
||||
|
||||
// display results
|
||||
Serial.printf_P(PSTR("Accel X:%4.2f \t Y:%4.2f \t Z:%4.2f \t len:%4.2f \t Gyro X:%4.2f \t Y:%4.2f \t Z:%4.2f \t Temp:%4.2f\n"),
|
||||
accel.x, accel.y, accel.z, length, gyro.x, gyro.y, gyro.z, temperature);
|
||||
}
|
||||
|
||||
// clear user input
|
||||
while( Serial.available() ) {
|
||||
Serial.read();
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user