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
* 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
@ -176,10 +176,20 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
AP_InertialSensor(),
_drdy_pin(NULL),
_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 )
{
if (_initialised) return _mpu9250_product_id;
@ -207,7 +217,7 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
do {
bool success = _hardware_init(sample_rate);
if (success) {
hal.scheduler->delay(5+2);
hal.scheduler->delay(10);
if (!_spi_sem->take(100)) {
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_data_transaction requires the spi semaphore to be taken by
* its caller. */
_last_sample_time_micros = hal.scheduler->micros();
hal.scheduler->delay(10);
if (_spi_sem->take(100)) {
_read_data_transaction();
@ -247,8 +256,23 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
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)
{
if (_sample_available()) {
@ -256,7 +280,11 @@ bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms)
}
uint32_t start = hal.scheduler->millis();
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()) {
return true;
}
@ -264,9 +292,11 @@ bool AP_InertialSensor_MPU9250::wait_for_sample(uint16_t timeout_ms)
return false;
}
/*
update the accel and gyro vectors
*/
bool AP_InertialSensor_MPU9250::update( void )
{
// wait for at least 1 sample
if (!wait_for_sample(1000)) {
return false;
}
@ -275,20 +305,16 @@ bool AP_InertialSensor_MPU9250::update( void )
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
_accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z);
_num_samples = _sum_count;
_accel_sum.zero();
_gyro_sum.zero();
_sum_count = 0;
_gyro[0] = _gyro_filtered;
_accel[0] = _accel_filtered;
hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation);
_gyro[0] *= _gyro_scale / _num_samples;
_gyro[0] *= GYRO_SCALE;
_gyro[0] -= _gyro_offset[0];
_accel[0].rotate(_board_orientation);
_accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples;
_accel[0] *= MPU9250_ACCEL_SCALE_1G;
// rotate for bbone default
_accel[0].rotate(ROTATION_ROLL_180_YAW_90);
@ -307,15 +333,12 @@ bool AP_InertialSensor_MPU9250::update( void )
_accel[0] -= _accel_offset[0];
if (_last_filter_hz != _mpu6000_filter) {
if (_spi_sem->take(10)) {
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
_set_filter_register(_mpu6000_filter, 0);
_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH);
_error_count = 0;
_spi_sem->give();
}
_set_filter(_mpu6000_filter);
_last_filter_hz = _mpu6000_filter;
}
_have_sample_available = false;
return true;
}
@ -336,44 +359,33 @@ bool AP_InertialSensor_MPU9250::_data_ready()
return (status & BIT_RAW_RDY_INT) != 0;
}
/**
* Timer process to poll for new data from the MPU9250.
*/
void AP_InertialSensor_MPU9250::_poll_data(void)
{
if (hal.scheduler->in_timerprocess()) {
if (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling wait_for_sample() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
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 (!_spi_sem->take_nonblocking()) {
/*
the semaphore being busy is an expected condition when the
mainline code is calling wait_for_sample() which will
grab the semaphore. We return now and rely on the mainline
code grabbing the latest sample.
*/
return;
}
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 */
struct PACKED {
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]))
_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) {
// rollover - v unlikely
_accel_sum.zero();
_gyro_sum.zero();
}
_accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
_accel_filter_y.apply(int16_val(rx.v, 0)),
_accel_filter_z.apply(-int16_val(rx.v, 2)));
_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 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[1] = 0;
_spi->transaction(tx, rx, 2);
return rx[1];
}
/*
write an 8 bit register
*/
void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val)
{
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;
// choose filtering frequency
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_hz == 0) {
filter_hz = _default_filter_hz;
}
if (filter != 0) {
_last_filter_hz = filter_hz;
_accel_filter_x.set_cutoff_frequency(1000, 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)
{
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
hal.scheduler->delay(1);
// Disable I2C bus (recommended on datasheet)
_register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1);
uint8_t default_filter;
// sample rate and filtering
// to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate
switch (sample_rate) {
case RATE_50HZ:
// this is used for plane and rover, where noise resistance is
// more important than update rate. Tests on an aerobatic plane
// show that 10Hz is fine, and makes it very noise resistant
default_filter = BITS_DLPF_CFG_10HZ;
_sample_shift = 2;
_default_filter_hz = 15;
_sample_time_usec = 20000;
break;
case RATE_100HZ:
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 1;
_default_filter_hz = 30;
_sample_time_usec = 10000;
break;
case RATE_200HZ:
_default_filter_hz = 30;
_sample_time_usec = 5000;
break;
case RATE_400HZ:
default:
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 0;
_default_filter_hz = 30;
_sample_time_usec = 2500;
break;
}
_set_filter_register(_mpu6000_filter, default_filter);
// set sample rate to 200Hz, and use _sample_divider to give
// the requested rate to the application
_register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
hal.scheduler->delay(1);
// used a fixed filter of 42Hz on the sensor, then filter using
// the 2-pole software filter
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ);
// 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
hal.scheduler->delay(1);
// RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 8g
_register_write(MPUREG_ACCEL_CONFIG,2<<3);
hal.scheduler->delay(1);
// configure interrupt to fire when new data arrives
_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
// until we clear the interrupt
@ -585,14 +563,6 @@ float AP_InertialSensor_MPU9250::get_gyro_drift_rate(void)
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
// dump all config registers - used for debug
void AP_InertialSensor_MPU9250::_dump_registers(void)
@ -610,12 +580,12 @@ void AP_InertialSensor_MPU9250::_dump_registers(void)
#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
{
// the sensor runs at 200Hz
return 0.005 * _num_samples;
return _sample_time_usec * 1.0e-6f;
}
#endif // CONFIG_HAL_BOARD

51
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -7,6 +7,8 @@
#include <AP_HAL.h>
#include <AP_Math.h>
#include <AP_Progmem.h>
#include <Filter.h>
#include <LowPassFilter2p.h>
#include "AP_InertialSensor.h"
// 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
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:
uint16_t _init_sensor( Sample_rate sample_rate );
AP_HAL::DigitalSource *_drdy_pin;
bool _sample_available();
void _read_data_transaction();
bool _data_ready();
void _poll_data(void);
uint8_t _register_read( uint8_t reg );
void _register_write( uint8_t reg, uint8_t val );
bool _hardware_init(Sample_rate sample_rate);
bool _sample_available();
AP_HAL::SPIDeviceDriver *_spi;
AP_HAL::Semaphore *_spi_sem;
uint16_t _num_samples;
static const float _gyro_scale;
uint32_t _last_sample_time_micros;
uint32_t _sample_time_usec;
uint32_t _last_sample_usec;
// ensure we can't initialise twice
bool _initialised;
bool _initialised;
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
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
// the sum of the values since last read
Vector3l _accel_sum;
Vector3l _gyro_sum;
volatile int16_t _sum_count;
// Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x;
LowPassFilter2p _accel_filter_y;
LowPassFilter2p _accel_filter_z;
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: