zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: implement a new strategy for fast sampling 

this moves to using a 1p filter on the high rate data, followed by
averaging down to 1kHz then a 2p filter to apply configured cutoff
frequency.

It also fixes the FIFO reset to not cause data corruption. We need to
disable all FIFO channels before doing the reset, and wait for the
FIFO to stop in the sensor.

Finally it moves sampling of the MPU6000 and MPU9250 into the main
thread. That significantly improves scheduling performance as we no
longer get long FIFO SPI transfers happening during other tasks. All
transfers happen at the start of the fast loop. That makes timing much
more predictable.

Thanks to Leonard and Paul for help with this design!
master
Andrew Tridgell 8 years ago
parent
b165e025d9
commit
d9c8db7024
5 changed files with 293 additions and 244 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
  2. 240
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
  3. 33
      libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
  4. 227
      libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
  5. 33
      libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
Unescape View File

@ -269,7 +269,7 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem @@ -269,7 +269,7 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem
*/
void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
{
if (!_sem->take_nonblocking()) {
if (!_sem->take(0)) {
return;
}
@ -292,7 +292,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) @@ -292,7 +292,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance)
*/
void AP_InertialSensor_Backend::update_accel(uint8_t instance)
{
if (!_sem->take_nonblocking()) {
if (!_sem->take(0)) {
return;
}

240
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
Unescape View File

@ -21,6 +21,8 @@ extern const AP_HAL::HAL& hal; @@ -21,6 +21,8 @@ extern const AP_HAL::HAL& hal;
#endif
#endif
#define debug(fmt, args ...) do {printf("MPU6000: " fmt "\n", ## args); } while(0)
/*
EXT_SYNC allows for frame synchronisation with an external device
such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
@ -64,6 +66,7 @@ extern const AP_HAL::HAL& hal; @@ -64,6 +66,7 @@ extern const AP_HAL::HAL& hal;
# define MPUREG_CONFIG_EXT_SYNC_AX 0x05
# define MPUREG_CONFIG_EXT_SYNC_AY 0x06
# define MPUREG_CONFIG_EXT_SYNC_AZ 0x07
# define MPUREG_CONFIG_FIFO_MODE_STOP 0x40
#define MPUREG_GYRO_CONFIG 0x1B
// bit definitions for MPUREG_GYRO_CONFIG
# define BITS_GYRO_FS_250DPS 0x00
@ -313,27 +316,33 @@ bool AP_InertialSensor_MPU6000::_init() @@ -313,27 +316,33 @@ bool AP_InertialSensor_MPU6000::_init()
bool success = _hardware_init();
#if MPU6000_DEBUG
_dump_registers();
#endif
return success;
}
void AP_InertialSensor_MPU6000::_fifo_reset()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
_register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
hal.scheduler->delay_microseconds(1);
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
void AP_InertialSensor_MPU6000::_fifo_enable()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
_register_write(MPUREG_FIFO_EN, 0);
hal.scheduler->delay_microseconds(1);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
hal.scheduler->delay_microseconds(1);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN,
true);
_fifo_reset();
hal.scheduler->delay(1);
}
@ -358,6 +367,10 @@ void AP_InertialSensor_MPU6000::start() @@ -358,6 +367,10 @@ void AP_InertialSensor_MPU6000::start()
// always use FIFO
_fifo_enable();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000));
// setup ODR and on-sensor filtering
_set_filter_register();
@ -408,10 +421,6 @@ void AP_InertialSensor_MPU6000::start() @@ -408,10 +421,6 @@ void AP_InertialSensor_MPU6000::start()
_dev->get_semaphore()->give();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU6000));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU6000));
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
@ -421,13 +430,18 @@ void AP_InertialSensor_MPU6000::start() @@ -421,13 +430,18 @@ void AP_InertialSensor_MPU6000::start()
if (_fifo_buffer == nullptr) {
AP_HAL::panic("MPU6000: Unable to allocate FIFO buffer");
}
// start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
if (get_sample_rate_hz() >= 400) {
_use_accumulate = true;
} else {
// start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
}
}
/*
process any
publish any pending data
*/
bool AP_InertialSensor_MPU6000::update()
{
@ -439,6 +453,17 @@ bool AP_InertialSensor_MPU6000::update() @@ -439,6 +453,17 @@ bool AP_InertialSensor_MPU6000::update()
return true;
}
/*
accumulate new samples
*/
void AP_InertialSensor_MPU6000::accumulate()
{
if (_use_accumulate && _dev->get_semaphore()->take(0)) {
_poll_data();
_dev->get_semaphore()->give();
}
}
AuxiliaryBus *AP_InertialSensor_MPU6000::get_auxiliary_bus()
{
if (_auxiliary_bus) {
@ -476,10 +501,10 @@ bool AP_InertialSensor_MPU6000::_poll_data() @@ -476,10 +501,10 @@ bool AP_InertialSensor_MPU6000::_poll_data()
return true;
}
void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
bool AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
{
for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
const uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
Vector3f accel, gyro;
bool fsync_set = false;
@ -492,9 +517,13 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) @@ -492,9 +517,13 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
-int16_val(data, 2));
accel *= _accel_scale;
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
_last_temp = temp;
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d", raw_temp, t2);
_fifo_reset();
return false;
}
float temp = t2/340.0 + 36.53;
gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4),
@ -509,85 +538,83 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples) @@ -509,85 +538,83 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
_temp_filtered = _temp_filter.apply(temp);
}
return true;
}
void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
/*
when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
To filter this we first apply a 1p low pass filter at 188Hz, then we
average over 8 samples to bring the data rate down to 1kHz. This
gives very good aliasing rejection at frequencies well above what
can be handled with 1kHz sample rates.
*/
bool AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
{
float tsum = 0;
int32_t tsum = 0;
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
bool clipped = false;
bool ret = true;
for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
Vector3l a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
if (abs(a.x) > clip_limit ||
abs(a.y) > clip_limit ||
abs(a.z) > clip_limit) {
clipped = true;
const uint8_t *data = samples + MPU6000_SAMPLE_SIZE * i;
// use temperatue to detect FIFO corruption
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2);
_fifo_reset();
ret = false;
break;
}
Vector3l g(int16_val(data, 5),
tsum += t2;
if ((_accum.count & 1) == 0) {
// accel data is at 4kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
if (fabsf(a.x) > clip_limit ||
fabsf(a.y) > clip_limit ||
fabsf(a.z) > clip_limit) {
clipped = true;
}
_accum.accel += _accum.accel_filter.apply(a);
}
Vector3f g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
_accum.accel += a;
_accum.gyro += g;
_accum.gyro += _accum.gyro_filter.apply(g);
_accum.count++;
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
tsum += temp;
_last_temp = temp;
}
if (clipped) {
increment_clip_count(_accel_instance);
}
_temp_filtered = _temp_filter.apply(tsum / n_samples);
float temp = (tsum/n_samples)/340.0 + 36.53;
_temp_filtered = _temp_filter.apply(temp);
if (_accum.count == MPU6000_MAX_FIFO_SAMPLES) {
float ascale = _accel_scale / _accum.count;
Vector3f accel(_accum.accel.x*ascale, _accum.accel.y*ascale, _accum.accel.z*ascale);
float ascale = _accel_scale / (MPU6000_MAX_FIFO_SAMPLES/2);
_accum.accel *= ascale;
float gscale = GYRO_SCALE / _accum.count;
Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale);
float gscale = GYRO_SCALE / MPU6000_MAX_FIFO_SAMPLES;
_accum.gyro *= gscale;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_accum.accel.zero();
_accum.gyro.zero();
_accum.count = 0;
}
}
/*
* check the FIFO integrity by cross-checking the temperature against
* the last FIFO reading
*/
void AP_InertialSensor_MPU6000::_check_temperature(void)
{
uint8_t rx[2];
if (!_block_read(MPUREG_TEMP_OUT_H, rx, 2)) {
return;
}
float temp = int16_val(rx, 0) / 340 + 36.53;
if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) {
// a 2 degree change in one sample is a highly likely
// sign of a FIFO alignment error
printf("MPU6000: FIFO temperature reset: %.2f %.2f\n",
(double)temp, (double)_last_temp);
_last_temp = temp;
_fifo_reset();
}
return ret;
}
void AP_InertialSensor_MPU6000::_read_fifo()
@ -595,9 +622,11 @@ void AP_InertialSensor_MPU6000::_read_fifo() @@ -595,9 +622,11 @@ void AP_InertialSensor_MPU6000::_read_fifo()
uint8_t n_samples;
uint16_t bytes_read;
uint8_t *rx = _fifo_buffer;
int16_t raw_temp;
uint8_t trx[2];
bool need_reset = false;
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
hal.console->printf("MPU60x0: error in fifo read\n");
goto check_registers;
}
@ -609,6 +638,26 @@ void AP_InertialSensor_MPU6000::_read_fifo() @@ -609,6 +638,26 @@ void AP_InertialSensor_MPU6000::_read_fifo()
goto check_registers;
}
/*
fetch temperature in order to detect FIFO sync errors
*/
if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
return;
}
raw_temp = int16_val(trx, 0);
/*
testing has shown that if we have more than 32 samples in the
FIFO then some of those samples will be corrupt. It always is
the ones at the end of the FIFO, so clear those with a reset
once we've read the first 24. Reading 24 gives us the normal
number of samples for fast sampling at 400Hz
*/
if (n_samples > 32) {
need_reset = true;
n_samples = 24;
}
while (n_samples > 0) {
uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES - _accum.count);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU6000_SAMPLE_SIZE)) {
@ -617,23 +666,23 @@ void AP_InertialSensor_MPU6000::_read_fifo() @@ -617,23 +666,23 @@ void AP_InertialSensor_MPU6000::_read_fifo()
}
if (_fast_sampling) {
_accumulate_fast_sampling(rx, n);
if (!_accumulate_fast_sampling(rx, n, raw_temp)) {
debug("stop at %u of %u", n_samples, bytes_read/MPU6000_SAMPLE_SIZE);
break;
}
} else {
_accumulate(rx, n);
if (!_accumulate(rx, n, raw_temp)) {
break;
}
}
n_samples -= n;
}
if (bytes_read > MPU6000_SAMPLE_SIZE * 35) {
printf("MPU60x0: fifo reset\n");
if (need_reset) {
//debug("fifo reset n_samples %u", bytes_read/MPU6000_SAMPLE_SIZE);
_fifo_reset();
}
if (_temp_counter++ == 255) {
// check FIFO integrity every 0.25s
_check_temperature();
}
check_registers:
if (_reg_check_counter++ == 10) {
_reg_check_counter = 0;
@ -680,6 +729,9 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void) @@ -680,6 +729,9 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void)
if (enable_fast_sampling(_accel_instance)) {
_fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
if (_fast_sampling) {
hal.console->printf("MPU6000: enabled fast sampling\n");
}
}
if (_fast_sampling) {
@ -689,6 +741,8 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void) @@ -689,6 +741,8 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void)
// limit to 1kHz if not on SPI
config |= BITS_DLPF_CFG_188HZ;
}
config |= MPUREG_CONFIG_FIFO_MODE_STOP;
_register_write(MPUREG_CONFIG, config, true);
if (_is_icm_device) {
@ -776,10 +830,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) @@ -776,10 +830,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
if (_data_ready()) {
break;
}
#if MPU6000_DEBUG
_dump_registers();
#endif
}
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
@ -798,28 +848,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void) @@ -798,28 +848,6 @@ bool AP_InertialSensor_MPU6000::_hardware_init(void)
return true;
}
#if MPU6000_DEBUG
// dump all config registers - used for debug
void AP_InertialSensor_MPU6000::_dump_registers(void)
{
hal.console->println("MPU6000 registers");
if (!_dev->get_semaphore()->take(0)) {
return;
}
for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) {
uint8_t v = _register_read(reg);
hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) {
hal.console->println();
}
}
hal.console->println();
_dev->get_semaphore()->give();
}
#endif
AP_MPU6000_AuxiliaryBusSlave::AP_MPU6000_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
uint8_t instance)
: AuxiliaryBusSlave(bus, addr, instance)

33
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
Unescape View File

@ -15,9 +15,6 @@ @@ -15,9 +15,6 @@
#include "AP_InertialSensor_Backend.h"
#include "AuxiliaryBus.h"
// enable debug to see a register dump on startup
#define MPU6000_DEBUG 0
class AP_MPU6000_AuxiliaryBus;
class AP_MPU6000_AuxiliaryBusSlave;
@ -42,7 +39,8 @@ public: @@ -42,7 +39,8 @@ public:
enum Rotation rotation = ROTATION_NONE);
/* update accel and gyro state */
bool update();
bool update() override;
void accumulate() override;
/*
* Return an AuxiliaryBus if the bus driver allows it
@ -56,10 +54,6 @@ private: @@ -56,10 +54,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
#if MPU6000_DEBUG
void _dump_registers();
#endif
/* Initialize sensor*/
bool _init();
bool _hardware_init();
@ -85,9 +79,8 @@ private: @@ -85,9 +79,8 @@ private:
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
void _accumulate(uint8_t *samples, uint8_t n_samples);
void _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
void _check_temperature(void);
bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
@ -110,11 +103,10 @@ private: @@ -110,11 +103,10 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
// last temperature reading, used to detect FIFO errors
float _last_temp;
uint8_t _temp_counter;
// are we using accumulate for sensor reading or a bus callback?
bool _use_accumulate;
// has master i2c been enabled?
bool _master_i2c_enable;
@ -123,11 +115,16 @@ private: @@ -123,11 +115,16 @@ private:
uint8_t _reg_check_counter;
// accumulators for fast sampling
/*
accumulators for fast sampling
See description in _accumulate_fast_sampling()
*/
struct {
Vector3l accel;
Vector3l gyro;
Vector3f accel;
Vector3f gyro;
uint8_t count;
LowPassFilterVector3f accel_filter{4000, 188};
LowPassFilterVector3f gyro_filter{8000, 188};
} _accum;
};

227
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
Unescape View File

@ -22,6 +22,8 @@ @@ -22,6 +22,8 @@
#include <AP_HAL/GPIO.h>
#define debug(fmt, args ...) do {printf("MPU9250: " fmt "\n", ## args); } while(0)
extern const AP_HAL::HAL &hal;
// MPU9250 accelerometer scaling for 16g range
@ -57,6 +59,7 @@ extern const AP_HAL::HAL &hal; @@ -57,6 +59,7 @@ extern const AP_HAL::HAL &hal;
# define MPUREG_SMPLRT_100HZ 0x09
# define MPUREG_SMPLRT_50HZ 0x13
#define MPUREG_CONFIG 0x1A
#define MPUREG_CONFIG_FIFO_MODE_STOP 0x40
#define MPUREG_GYRO_CONFIG 0x1B
// bit definitions for MPUREG_GYRO_CONFIG
# define BITS_GYRO_FS_250DPS 0x00
@ -263,28 +266,31 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i @@ -263,28 +266,31 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
bool AP_InertialSensor_MPU9250::_init()
{
bool success = _hardware_init();
#if MPU9250_DEBUG
_dump_registers();
#endif
return success;
}
void AP_InertialSensor_MPU9250::_fifo_reset()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
_register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN);
hal.scheduler->delay_microseconds(1);
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
void AP_InertialSensor_MPU9250::_fifo_enable()
{
uint8_t user_ctrl = _master_i2c_enable?BIT_USER_CTRL_I2C_MST_EN:0;
_register_write(MPUREG_FIFO_EN, 0);
_register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_RESET | BIT_USER_CTRL_FIFO_EN);
_register_write(MPUREG_FIFO_EN, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN,
true);
_fifo_reset();
hal.scheduler->delay(1);
}
@ -309,15 +315,20 @@ void AP_InertialSensor_MPU9250::start() @@ -309,15 +315,20 @@ void AP_InertialSensor_MPU9250::start()
// always use FIFO
_fifo_enable();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU9250));
if (enable_fast_sampling(_accel_instance) && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
_fast_sampling = true;
hal.console->printf("MPU9250: enabled fast sampling\n");
}
if (_fast_sampling) {
// setup for fast sampling
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2, true);
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2 | MPUREG_CONFIG_FIFO_MODE_STOP, true);
} else {
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_188HZ, true);
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_188HZ | MPUREG_CONFIG_FIFO_MODE_STOP, true);
}
// set sample rate to 1kHz, and use the 2 pole filter to give the
@ -353,10 +364,6 @@ void AP_InertialSensor_MPU9250::start() @@ -353,10 +364,6 @@ void AP_InertialSensor_MPU9250::start()
_dev->get_semaphore()->give();
// grab the used instances
_gyro_instance = _imu.register_gyro(1000, _dev->get_bus_id_devtype(DEVTYPE_GYR_MPU9250));
_accel_instance = _imu.register_accel(1000, _dev->get_bus_id_devtype(DEVTYPE_ACC_MPU9250));
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
@ -365,9 +372,13 @@ void AP_InertialSensor_MPU9250::start() @@ -365,9 +372,13 @@ void AP_InertialSensor_MPU9250::start()
if (_fifo_buffer == nullptr) {
AP_HAL::panic("MPU9250: Unable to allocate FIFO buffer");
}
// start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));
if (get_sample_rate_hz() >= 400) {
_use_accumulate = true;
} else {
// start the timer process to read samples
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool));
}
}
/*
@ -383,6 +394,18 @@ bool AP_InertialSensor_MPU9250::update() @@ -383,6 +394,18 @@ bool AP_InertialSensor_MPU9250::update()
return true;
}
/*
accumulate new samples
*/
void AP_InertialSensor_MPU9250::accumulate()
{
if (_use_accumulate && _dev->get_semaphore()->take(0)) {
_read_sample();
_dev->get_semaphore()->give();
}
}
AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus()
{
if (_auxiliary_bus) {
@ -411,10 +434,10 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status) @@ -411,10 +434,10 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status)
}
void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
bool AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
{
for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
const uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
Vector3f accel, gyro;
accel = Vector3f(int16_val(data, 1),
@ -422,9 +445,13 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples) @@ -422,9 +445,13 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
-int16_val(data, 2));
accel *= MPU9250_ACCEL_SCALE_1G;
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
_last_temp = temp;
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d %d", raw_temp, t2, raw_temp-t2);
_fifo_reset();
return false;
}
float temp = t2/340 + 36.53;
gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4),
@ -439,87 +466,85 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples) @@ -439,87 +466,85 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *samples, uint8_t n_samples)
_temp_filtered = _temp_filter.apply(temp);
}
return true;
}
void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
/*
when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
To filter this we first apply a 1p low pass filter at 188Hz, then we
average over 8 samples to bring the data rate down to 1kHz. This
gives very good aliasing rejection at frequencies well above what
can be handled with 1kHz sample rates.
*/
bool AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp)
{
float tsum = 0;
int32_t tsum = 0;
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / MPU9250_ACCEL_SCALE_1G;
bool clipped = false;
bool ret = true;
for (uint8_t i = 0; i < n_samples; i++) {
uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
Vector3l a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
if (abs(a.x) > clip_limit ||
abs(a.y) > clip_limit ||
abs(a.z) > clip_limit) {
clipped = true;
const uint8_t *data = samples + MPU9250_SAMPLE_SIZE * i;
// use temperatue to detect FIFO corruption
int16_t t2 = int16_val(data, 3);
if (abs(t2 - raw_temp) > 400) {
debug("temp reset %d %d %d", raw_temp, t2, raw_temp - t2);
_fifo_reset();
ret = false;
break;
}
tsum += t2;
if ((_accum.count & 1) == 0) {
// accels are at 4kHz not 8kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
if (fabsf(a.x) > clip_limit ||
fabsf(a.y) > clip_limit ||
fabsf(a.z) > clip_limit) {
clipped = true;
}
_accum.accel += _accum.accel_filter.apply(a);
}
Vector3l g(int16_val(data, 5),
Vector3f g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
_accum.accel += a;
_accum.gyro += g;
_accum.gyro += _accum.gyro_filter.apply(g);
_accum.count++;
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
tsum += temp;
_last_temp = temp;
}
if (clipped) {
increment_clip_count(_accel_instance);
}
_temp_filtered = _temp_filter.apply(tsum / n_samples);
float temp = (tsum/n_samples)/340.0 + 36.53;
_temp_filtered = _temp_filter.apply(temp);
if (_accum.count == MPU9250_MAX_FIFO_SAMPLES) {
float ascale = MPU9250_ACCEL_SCALE_1G / _accum.count;
Vector3f accel(_accum.accel.x*ascale, _accum.accel.y*ascale, _accum.accel.z*ascale);
float ascale = MPU9250_ACCEL_SCALE_1G / (MPU9250_MAX_FIFO_SAMPLES/2);
_accum.accel *= ascale;
float gscale = GYRO_SCALE / _accum.count;
Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale);
float gscale = GYRO_SCALE / MPU9250_MAX_FIFO_SAMPLES;
_accum.gyro *= gscale;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_accum.accel.zero();
_accum.gyro.zero();
_accum.count = 0;
}
return ret;
}
/*
* check the FIFO integrity by cross-checking the temperature against
* the last FIFO reading
*/
void AP_InertialSensor_MPU9250::_check_temperature(void)
{
uint8_t rx[2];
if (!_block_read(MPUREG_TEMP_OUT_H, rx, 2)) {
return;
}
float temp = int16_val(rx, 0) / 340 + 36.53;
if (fabsf(_last_temp - temp) > 2 && !is_zero(_last_temp)) {
// a 2 degree change in one sample is a highly likely
// sign of a FIFO alignment error
printf("MPU9250: FIFO temperature reset: %.2f %.2f\n",
(double)temp, (double)_last_temp);
_last_temp = temp;
_fifo_reset();
}
}
/*
* read from the data registers and update filtered data
*/
@ -528,9 +553,11 @@ bool AP_InertialSensor_MPU9250::_read_sample() @@ -528,9 +553,11 @@ bool AP_InertialSensor_MPU9250::_read_sample()
uint8_t n_samples;
uint16_t bytes_read;
uint8_t *rx = _fifo_buffer;
int16_t raw_temp;
uint8_t trx[2];
bool need_reset = false;
if (!_block_read(MPUREG_FIFO_COUNTH, rx, 2)) {
hal.console->printf("MPU9250: error in fifo read\n");
goto check_registers;
}
@ -542,6 +569,26 @@ bool AP_InertialSensor_MPU9250::_read_sample() @@ -542,6 +569,26 @@ bool AP_InertialSensor_MPU9250::_read_sample()
goto check_registers;
}
/*
fetch temperature in order to detect FIFO sync errors
*/
if (!_block_read(MPUREG_TEMP_OUT_H, trx, 2)) {
return true;
}
raw_temp = int16_val(trx, 0);
/*
testing has shown that if we have more than 32 samples in the
FIFO then some of those samples will be corrupt. It always is
the ones at the end of the FIFO, so clear those with a reset
once we've read the first 24. Reading 24 gives us the normal
number of samples for fast sampling at 400Hz
*/
if (n_samples > 32) {
need_reset = true;
n_samples = 24;
}
while (n_samples > 0) {
uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES - _accum.count, n_samples);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU9250_SAMPLE_SIZE)) {
@ -550,23 +597,23 @@ bool AP_InertialSensor_MPU9250::_read_sample() @@ -550,23 +597,23 @@ bool AP_InertialSensor_MPU9250::_read_sample()
}
if (_fast_sampling) {
_accumulate_fast_sampling(rx, n);
if (!_accumulate_fast_sampling(rx, n, raw_temp)) {
debug("stop at %u of %u", n_samples, bytes_read/MPU9250_SAMPLE_SIZE);
break;
}
} else {
_accumulate(rx, n);
if (!_accumulate(rx, n, raw_temp)) {
break;
}
}
n_samples -= n;
}
if (bytes_read > MPU9250_SAMPLE_SIZE * 35) {
printf("MPU9250: fifo reset\n");
if (need_reset) {
//debug("fifo reset %u", bytes_read/MPU9250_SAMPLE_SIZE);
_fifo_reset();
}
if (_temp_counter++ == 255) {
// check FIFO integrity every 0.25s
_check_temperature();
}
check_registers:
if (_reg_check_counter++ == 10) {
_reg_check_counter = 0;
@ -655,10 +702,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) @@ -655,10 +702,6 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void)
if (_data_ready()) {
break;
}
#if MPU9250_DEBUG
_dump_registers();
#endif
}
if (tries == 5) {
hal.console->println("Failed to boot MPU9250 5 times");
@ -677,22 +720,6 @@ fail_whoami: @@ -677,22 +720,6 @@ fail_whoami:
return false;
}
#if MPU9250_DEBUG
// dump all config registers - used for debug
void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus)
{
hal.console->println("MPU9250 registers");
for (uint8_t reg=0; reg<=126; reg++) {
uint8_t v = _register_read(bus, reg);
hal.console->printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg - (MPUREG_PRODUCT_ID-1)) % 16 == 0) {
hal.console->println();
}
}
hal.console->println();
}
#endif
AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
uint8_t instance)
: AuxiliaryBusSlave(bus, addr, instance)

33
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
Unescape View File

@ -16,9 +16,6 @@ @@ -16,9 +16,6 @@
class AP_MPU9250_AuxiliaryBus;
class AP_MPU9250_AuxiliaryBusSlave;
// enable debug to see a register dump on startup
#define MPU9250_DEBUG 0
class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend
{
friend AP_MPU9250_AuxiliaryBus;
@ -40,7 +37,8 @@ public: @@ -40,7 +37,8 @@ public:
enum Rotation rotation = ROTATION_NONE);
/* update accel and gyro state */
bool update();
bool update() override;
void accumulate() override;
/*
* Return an AuxiliaryBus if the bus driver allows it
@ -54,10 +52,6 @@ private: @@ -54,10 +52,6 @@ private:
AP_HAL::OwnPtr<AP_HAL::Device> dev,
enum Rotation rotation);
#if MPU9250_DEBUG
static void _dump_registers();
#endif
bool _init();
bool _hardware_init();
@ -80,9 +74,8 @@ private: @@ -80,9 +74,8 @@ private:
uint8_t _register_read(uint8_t reg);
void _register_write(uint8_t reg, uint8_t val, bool checked=false);
void _accumulate(uint8_t *samples, uint8_t n_samples);
void _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples);
void _check_temperature(void);
bool _accumulate(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
bool _accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples, int16_t raw_temp);
// instance numbers of accel and gyro data
uint8_t _gyro_instance;
@ -99,23 +92,27 @@ private: @@ -99,23 +92,27 @@ private:
// are we doing more than 1kHz sampling?
bool _fast_sampling;
// are we using accumulate or timer callback?
bool _use_accumulate;
// has master i2c been enabled?
bool _master_i2c_enable;
// last temperature reading, used to detect FIFO errors
float _last_temp;
uint8_t _temp_counter;
// buffer for fifo read
uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
// accumulators for fast sampling
/*
accumulators for fast sampling
See description in _accumulate_fast_sampling()
*/
struct {
Vector3l accel;
Vector3l gyro;
Vector3f accel;
Vector3f gyro;
uint8_t count;
LowPassFilterVector3f accel_filter{8000, 188};
LowPassFilterVector3f gyro_filter{8000, 188};
} _accum;
};

Write Preview
Loading…
Cancel
Save