From 49de46a548d6ce263c51778cef619ff679d2fd22 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Mon, 5 Nov 2012 13:27:03 +0900 Subject: [PATCH] AP_InertialSensor: merge in calibration features from IMU library add gauss-newton method of accelerometer calibration --- .../AP_InertialSensor/AP_InertialSensor.cpp | 463 ++++++++++++++++++ .../AP_InertialSensor/AP_InertialSensor.h | 139 +++++- .../AP_InertialSensor_MPU6000.cpp | 132 +++-- .../AP_InertialSensor_MPU6000.h | 30 +- .../AP_InertialSensor_Oilpan.cpp | 99 +--- .../AP_InertialSensor_Oilpan.h | 30 +- .../AP_InertialSensor_Stub.cpp | 15 +- .../AP_InertialSensor_Stub.h | 9 +- .../examples/MPU6000/MPU6000.pde | 169 ++++++- 9 files changed, 836 insertions(+), 250 deletions(-) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor.cpp diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp new file mode 100644 index 0000000000..cda72d4bbe --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -0,0 +1,463 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include + +#include "AP_InertialSensor.h" + +#include +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include +#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 #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]; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h index 94bd77f06c..60b4e0b749 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h @@ -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" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp index e3ded8b07c..36b60a04ec 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp @@ -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) { diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h index f90cee2008..483c918259 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.h @@ -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__ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp index 7e90200902..181aff36bb 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.cpp @@ -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; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h index 6fd58a26b6..b2187c23ab 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Stub.h @@ -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(); }; diff --git a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde index bcebaa186d..2edd6d96a7 100644 --- a/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde +++ b/libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde @@ -8,15 +8,34 @@ #include #include #include +#include #include #include #include +#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(); + } +} \ No newline at end of file