aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: update MPU9250 driver for 1kHz operation

Browse Source 
use a time base sample wait, with 1kHz sampling
This commit is contained in:
Andrew Tridgell 2014-08-19 19:39:21 +10:00
parent 532e9aace4
commit 9e01c657e5
2 changed files with 135 additions and 164 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

248
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -162,7 +162,7 @@ extern const AP_HAL::HAL& hal;
* PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of * PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3) * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
*/ */
const float AP_InertialSensor_MPU9250::_gyro_scale = (0.0174532f / 16.4f); #define GYRO_SCALE (0.0174532f / 16.4f)
/* /*
* PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of * PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of
@ -176,10 +176,20 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
AP_InertialSensor(), AP_InertialSensor(),
_drdy_pin(NULL), _drdy_pin(NULL),
_initialised(false), _initialised(false),
_mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE) _mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE),
_last_filter_hz(-1),
_accel_filter_x(1000, 15),
_accel_filter_y(1000, 15),
_accel_filter_z(1000, 15),
_gyro_filter_x(1000, 15),
_gyro_filter_y(1000, 15),
_gyro_filter_z(1000, 15)
{ {
} }
/*
initialise the sensor
*/
uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate ) uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
{ {
if (_initialised) return _mpu9250_product_id; if (_initialised) return _mpu9250_product_id;
@ -207,7 +217,7 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
do { do {
bool success = _hardware_init(sample_rate); bool success = _hardware_init(sample_rate);
if (success) { if (success) {
hal.scheduler->delay(5+2); hal.scheduler->delay(10);
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore")); hal.scheduler->panic(PSTR("MPU9250: Unable to get semaphore"));
} }
@ -231,7 +241,6 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
/* read the first lot of data. /* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by * _read_data_transaction requires the spi semaphore to be taken by
* its caller. */ * its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10); hal.scheduler->delay(10);
if (_spi_sem->take(100)) { if (_spi_sem->take(100)) {
_read_data_transaction(); _read_data_transaction();
@ -247,8 +256,23 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
return _mpu9250_product_id; return _mpu9250_product_id;
} }
/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ /*
determine if a sample is available. We are using a time based
strategy, to avoid time sync issues with the sensor
*/
bool AP_InertialSensor_MPU9250::_sample_available()
{
uint32_t tnow = hal.scheduler->micros();
while (tnow - _last_sample_usec > _sample_time_usec) {
_have_sample_available = true;
_last_sample_usec += _sample_time_usec;
}
return _have_sample_available;
}
/*
wait for at least one sample to be available from the sensor
*/
bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms) bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms)
{ {
if (_sample_available()) { if (_sample_available()) {
@ -256,7 +280,11 @@ bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms)
} }
uint32_t start = hal.scheduler->millis(); uint32_t start = hal.scheduler->millis();
while ((hal.scheduler->millis() - start) < timeout_ms) { while ((hal.scheduler->millis() - start) < timeout_ms) {
hal.scheduler->delay_microseconds(100); uint32_t tnow = hal.scheduler->micros();
uint32_t tdelay = (_last_sample_usec + _sample_time_usec) - tnow;
if (tdelay < 100000) {
hal.scheduler->delay_microseconds(tdelay);
}
if (_sample_available()) { if (_sample_available()) {
return true; return true;
} }
@ -264,9 +292,11 @@ bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms)
return false; return false;
} }
/*
update the accel and gyro vectors
*/
bool AP_InertialSensor_MPU9250::update( void ) bool AP_InertialSensor_MPU9250::update( void )
{ {
// wait for at least 1 sample
if (!wait_for_sample(1000)) { if (!wait_for_sample(1000)) {
return false; return false;
} }
@ -275,20 +305,16 @@ bool AP_InertialSensor_MPU9250::update( void )
// disable timer procs for mininum time // disable timer procs for mininum time
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); _gyro[0] = _gyro_filtered;
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); _accel[0] = _accel_filtered;
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_sum_count = 0;
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation); _gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples; _gyro[0] *= GYRO_SCALE;
_gyro[0] -= _gyro_offset[0]; _gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation); _accel[0].rotate(_board_orientation);
_accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples; _accel[0] *= MPU9250_ACCEL_SCALE_1G;
// rotate for bbone default // rotate for bbone default
_accel[0].rotate(ROTATION_ROLL_180_YAW_90); _accel[0].rotate(ROTATION_ROLL_180_YAW_90);
@ -307,15 +333,12 @@ bool AP_InertialSensor_MPU9250::update( void )
_accel[0] -= _accel_offset[0]; _accel[0] -= _accel_offset[0];
if (_last_filter_hz != _mpu6000_filter) { if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) { _set_filter(_mpu6000_filter);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _last_filter_hz = _mpu6000_filter;
_set_filter_register(_mpu6000_filter, 0);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_error_count = 0;
_spi_sem->give();
}
} }
_have_sample_available = false;
return true; return true;
} }
@ -336,44 +359,33 @@ bool AP_InertialSensor_MPU9250::_data_ready()
return (status & BIT_RAW_RDY_INT) != 0; return (status & BIT_RAW_RDY_INT) != 0;
} }
/** /**
* Timer process to poll for new data from the MPU9250. * Timer process to poll for new data from the MPU9250.
*/ */
void AP_InertialSensor_MPU9250::_poll_data(void) void AP_InertialSensor_MPU9250::_poll_data(void)
{ {
if (hal.scheduler->in_timerprocess()) { if (!_spi_sem->take_nonblocking()) {
if (!_spi_sem->take_nonblocking()) { /*
/* the semaphore being busy is an expected condition when the
the semaphore being busy is an expected condition when the mainline code is calling wait_for_sample() which will
mainline code is calling wait_for_sample() which will grab the semaphore. We return now and rely on the mainline
grab the semaphore. We return now and rely on the mainline code grabbing the latest sample.
code grabbing the latest sample. */
*/ return;
return;
}
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
/* Synchronous read - take semaphore */
if (_spi_sem->take(10)) {
if (_data_ready()) {
_last_sample_time_micros = hal.scheduler->micros();
_read_data_transaction();
}
_spi_sem->give();
} else {
hal.scheduler->panic(
PSTR("PANIC: AP_InertialSensor_MPU9250::_poll_data "
"failed to take SPI semaphore synchronously"));
}
} }
if (_data_ready()) {
_read_data_transaction();
}
_spi_sem->give();
} }
void AP_InertialSensor_MPU9250::_read_data_transaction() { /*
read from the data registers and update filtered data
*/
void AP_InertialSensor_MPU9250::_read_data_transaction()
{
/* one resister address followed by seven 2-byte registers */ /* one resister address followed by seven 2-byte registers */
struct PACKED { struct PACKED {
uint8_t cmd; uint8_t cmd;
@ -390,38 +402,20 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() {
} }
} }
/*
detect a bad SPI bus transaction by looking for all 14 bytes
zero, or the wrong INT_STATUS register value. This is used to
detect a too high SPI bus speed.
*/
uint8_t i;
for (i=0; i<14; i++) {
if (rx.v[i] != 0) break;
}
if ((rx.int_status&~0x6) != (_drdy_pin==NULL?0:BIT_RAW_RDY_INT) || i == 14) {
// likely a bad bus transaction
if (++_error_count > 4) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
}
}
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) #define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
_accel_sum.x += int16_val(rx.v, 1);
_accel_sum.y += int16_val(rx.v, 0);
_accel_sum.z -= int16_val(rx.v, 2);
_gyro_sum.x += int16_val(rx.v, 5);
_gyro_sum.y += int16_val(rx.v, 4);
_gyro_sum.z -= int16_val(rx.v, 6);
_sum_count++;
if (_sum_count == 0) { _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
// rollover - v unlikely _accel_filter_y.apply(int16_val(rx.v, 0)),
_accel_sum.zero(); _accel_filter_z.apply(-int16_val(rx.v, 2)));
_gyro_sum.zero();
} _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)),
_gyro_filter_y.apply(int16_val(rx.v, 4)),
_gyro_filter_z.apply(-int16_val(rx.v, 6)));
} }
/*
read an 8 bit register
*/
uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg ) uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg )
{ {
uint8_t addr = reg | 0x80; // Set most significant bit uint8_t addr = reg | 0x80; // Set most significant bit
@ -432,10 +426,12 @@ uint8_t AP_InertialSensor_MPU9250::_register_read( uint8_t reg )
tx[0] = addr; tx[0] = addr;
tx[1] = 0; tx[1] = 0;
_spi->transaction(tx, rx, 2); _spi->transaction(tx, rx, 2);
return rx[1]; return rx[1];
} }
/*
write an 8 bit register
*/
void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
{ {
uint8_t tx[2]; uint8_t tx[2];
@ -447,38 +443,27 @@ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
} }
/* /*
set the DLPF filter frequency. Assumes caller has taken semaphore set the accel/gyro filter frequency
*/ */
void AP_InertialSensor_MPU9250::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) void AP_InertialSensor_MPU9250::_set_filter(uint8_t filter_hz)
{ {
uint8_t filter = default_filter; if (filter_hz == 0) {
// choose filtering frequency filter_hz = _default_filter_hz;
switch (filter_hz) {
case 5:
filter = BITS_DLPF_CFG_5HZ;
break;
case 10:
filter = BITS_DLPF_CFG_10HZ;
break;
case 20:
filter = BITS_DLPF_CFG_20HZ;
break;
case 42:
filter = BITS_DLPF_CFG_42HZ;
break;
case 98:
filter = BITS_DLPF_CFG_98HZ;
break;
} }
if (filter != 0) { _accel_filter_x.set_cutoff_frequency(1000, filter_hz);
_last_filter_hz = filter_hz; _accel_filter_y.set_cutoff_frequency(1000, filter_hz);
_accel_filter_z.set_cutoff_frequency(1000, filter_hz);
_register_write(MPUREG_CONFIG, filter); _gyro_filter_x.set_cutoff_frequency(1000, filter_hz);
} _gyro_filter_y.set_cutoff_frequency(1000, filter_hz);
_gyro_filter_z.set_cutoff_frequency(1000, filter_hz);
} }
/*
initialise the sensor configuration registers
*/
bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate) bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
{ {
if (!_spi_sem->take(100)) { if (!_spi_sem->take(100)) {
@ -515,54 +500,47 @@ bool AP_InertialSensor_MPU9250::_hardware_init(Sample_rate sample_rate)
} }
_register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
hal.scheduler->delay(1);
// Disable I2C bus (recommended on datasheet) // Disable I2C bus (recommended on datasheet)
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1);
uint8_t default_filter;
// sample rate and filtering // sample rate and filtering
// to minimise the effects of aliasing we choose a filter // to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate // that is less than half of the sample rate
switch (sample_rate) { switch (sample_rate) {
case RATE_50HZ: case RATE_50HZ:
// this is used for plane and rover, where noise resistance is _default_filter_hz = 15;
// more important than update rate. Tests on an aerobatic plane _sample_time_usec = 20000;
// show that 10Hz is fine, and makes it very noise resistant
default_filter = BITS_DLPF_CFG_10HZ;
_sample_shift = 2;
break; break;
case RATE_100HZ: case RATE_100HZ:
default_filter = BITS_DLPF_CFG_20HZ; _default_filter_hz = 30;
_sample_shift = 1; _sample_time_usec = 10000;
break; break;
case RATE_200HZ: case RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case RATE_400HZ:
default: default:
default_filter = BITS_DLPF_CFG_20HZ; _default_filter_hz = 30;
_sample_shift = 0; _sample_time_usec = 2500;
break; break;
} }
_set_filter_register(_mpu6000_filter, default_filter); // used a fixed filter of 42Hz on the sensor, then filter using
// the 2-pole software filter
// set sample rate to 200Hz, and use _sample_divider to give _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ);
// the requested rate to the application
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
hal.scheduler->delay(1);
// set sample rate to 1kHz, and use the 2 pole filter to give the
// desired rate
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_1000HZ);
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
hal.scheduler->delay(1);
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g
_register_write(MPUREG_ACCEL_CONFIG,2<<3); _register_write(MPUREG_ACCEL_CONFIG,2<<3);
hal.scheduler->delay(1);
// configure interrupt to fire when new data arrives // configure interrupt to fire when new data arrives
_register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
hal.scheduler->delay(1);
// clear interrupt on any read, and hold the data ready pin high // clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt // until we clear the interrupt
@ -585,14 +563,6 @@ float AP_InertialSensor_MPU9250::get_gyro_drift_rate(void)
return ToRad(0.5/60); return ToRad(0.5/60);
} }
// return true if a sample is available
bool AP_InertialSensor_MPU9250::_sample_available()
{
_poll_data();
return (_sum_count >> _sample_shift) > 0;
}
#if MPU9250_DEBUG #if MPU9250_DEBUG
// dump all config registers - used for debug // dump all config registers - used for debug
void AP_InertialSensor_MPU9250::_dump_registers(void) void AP_InertialSensor_MPU9250::_dump_registers(void)
@ -610,12 +580,12 @@ void AP_InertialSensor_MPU9250::_dump_registers(void)
#endif #endif
// get_delta_time returns the time period in seconds overwhich the sensor data was collected // get_delta_time returns the time period in seconds overwhich the
// sensor data was collected. We just use a constant time, to decouple
// the 9250 timing from the main scheduler
float AP_InertialSensor_MPU9250::get_delta_time() const float AP_InertialSensor_MPU9250::get_delta_time() const
{ {
// the sensor runs at 200Hz return _sample_time_usec * 1.0e-6f;
return 0.005 * _num_samples;
} }
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

51
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -7,6 +7,8 @@
#include <AP_HAL.h> #include <AP_HAL.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_Progmem.h> #include <AP_Progmem.h>
#include <Filter.h>
#include <LowPassFilter2p.h>
#include "AP_InertialSensor.h" #include "AP_InertialSensor.h"
// enable debug to see a register dump on startup // enable debug to see a register dump on startup
@ -28,52 +30,51 @@ public:
// get_delta_time returns the time period in seconds overwhich the sensor data was collected // get_delta_time returns the time period in seconds overwhich the sensor data was collected
float get_delta_time() const; float get_delta_time() const;
uint16_t error_count(void) const { return _error_count; }
bool healthy(void) const { return _error_count <= 4; }
bool get_gyro_health(uint8_t instance) const { return healthy(); }
bool get_accel_health(uint8_t instance) const { return healthy(); }
protected:
uint16_t _init_sensor( Sample_rate sample_rate );
private: private:
uint16_t _init_sensor( Sample_rate sample_rate );
AP_HAL::DigitalSource *_drdy_pin; AP_HAL::DigitalSource *_drdy_pin;
bool _sample_available();
void _read_data_transaction(); void _read_data_transaction();
bool _data_ready(); bool _data_ready();
void _poll_data(void); void _poll_data(void);
uint8_t _register_read( uint8_t reg ); uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val ); void _register_write( uint8_t reg, uint8_t val );
bool _hardware_init(Sample_rate sample_rate); bool _hardware_init(Sample_rate sample_rate);
bool _sample_available();
AP_HAL::SPIDeviceDriver *_spi; AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem; AP_HAL::Semaphore *_spi_sem;
uint16_t _num_samples; uint32_t _sample_time_usec;
static const float _gyro_scale; uint32_t _last_sample_usec;
uint32_t _last_sample_time_micros;
// ensure we can't initialise twice // ensure we can't initialise twice
bool _initialised; bool _initialised;
int16_t _mpu9250_product_id; int16_t _mpu9250_product_id;
// how many hardware samples before we report a sample to the caller
uint8_t _sample_shift;
// support for updating filter at runtime // support for updating filter at runtime
uint8_t _last_filter_hz; int16_t _last_filter_hz;
void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); // change the filter frequency
void _set_filter(uint8_t filter_hz);
uint16_t _error_count; // output of accel/gyro filters
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
// accumulation in timer - must be read with timer disabled // Low Pass filters for gyro and accel
// the sum of the values since last read LowPassFilter2p _accel_filter_x;
Vector3l _accel_sum; LowPassFilter2p _accel_filter_y;
Vector3l _gyro_sum; LowPassFilter2p _accel_filter_z;
volatile int16_t _sum_count; LowPassFilter2p _gyro_filter_x;
LowPassFilter2p _gyro_filter_y;
LowPassFilter2p _gyro_filter_z;
// do we currently have a sample pending?
bool _have_sample_available;
// default filter frequency when set to zero
uint8_t _default_filter_hz;
public: public: