Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added INS_FAST_SAMPLE parameter 

this allows enable/disable of fast sampling per IMU, making
experimentation easier.

It also fixes the fast sampling to always average over 8 samples, and
fixes the 9250 to use the correct accumulator when not doing fast
sampling
This commit is contained in:
Andrew Tridgell 2016-11-15 16:51:18 +11:00
parent 95134d87b0
commit 624178f3be
7 changed files with 98 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -418,11 +418,16 @@ const AP_Param::GroupInfo AP_InertialSensor::var_info[] = {
// @User: Advanced
AP_GROUPINFO("ACC3_ID", 35, AP_InertialSensor, _accel_id[2], 0),
// @Param: FAST_SAMPLE
// @DisplayName: Fast sampling mask
// @Description: Mask of IMUs to enable fast sampling on, if available
// @User: Advanced
AP_GROUPINFO("FAST_SAMPLE", 36, AP_InertialSensor, _fast_sampling_mask, 0),
/*
NOTE: parameter indexes have gaps above. When adding new
parameters check for conflicts carefully
*/
AP_GROUPEND
};
@ -691,32 +696,32 @@ AP_InertialSensor::detect_backends(void)
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
// older Pixhawk2 boards have the MPU6000 instead of MPU9250
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), true, ROTATION_PITCH_180))) {
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_EXT_NAME), ROTATION_PITCH_180))) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_EXT_NAME), ROTATION_PITCH_180));
}
_add_backend(AP_InertialSensor_LSM9DS0::probe(*this,
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_G_NAME),
hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_ROLL_180_YAW_270));
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_YAW_270))) {
if (!_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270))) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME), ROTATION_YAW_270));
}
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_NAME), ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_ROLL_180_YAW_90));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180_YAW_90));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PHMINI) {
// PHMINI uses ICM20608 on the ACCEL_MAG device and a MPU9250 on the old MPU6000 CS line
_add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_ICM20608_AM_NAME), ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, ROTATION_ROLL_180));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_ROLL_180));
} else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PH2SLIM) {
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), true, ROTATION_YAW_270));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
}
// also add any PX4 backends (eg. canbus sensors)
_add_backend(AP_InertialSensor_PX4::detect(*this));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI && defined(HAL_INS_DEFAULT_ROTATION)
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), false, HAL_INS_DEFAULT_ROTATION));
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), HAL_INS_DEFAULT_ROTATION));
#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI
_add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME)));
#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0

3
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -352,6 +352,9 @@ private:
// use for attitude, velocity, position estimates
AP_Int8 _use[INS_MAX_INSTANCES];
// control enable of fast sampling
AP_Int8 _fast_sampling_mask;
// board orientation from AHRS
enum Rotation _board_orientation;

5
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -186,6 +186,11 @@ protected:
void increment_clip_count(uint8_t instance) {
_imu._accel_clip_count[instance]++;
}
// should fast sampling be enabled on this IMU?
bool enable_fast_sampling(uint8_t instance) {
return (_imu._fast_sampling_mask & (1U<<instance)) != 0;
}
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor

53
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

@ -513,7 +513,6 @@ void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)
void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint8_t n_samples)
{
Vector3l asum, gsum;
float tsum = 0;
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / _accel_scale;
bool clipped = false;
@ -528,10 +527,13 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
abs(a.z) > clip_limit) {
clipped = true;
}
asum += a;
gsum += Vector3l(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
Vector3l g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
_accum.accel += a;
_accum.gyro += g;
_accum.count++;
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
@ -542,20 +544,27 @@ void AP_InertialSensor_MPU6000::_accumulate_fast_sampling(uint8_t *samples, uint
if (clipped) {
increment_clip_count(_accel_instance);
}
float ascale = _accel_scale / n_samples;
Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale);
float gscale = GYRO_SCALE / n_samples;
Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale);
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(tsum / n_samples);
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 gscale = GYRO_SCALE / _accum.count;
Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale);
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_accum.accel.zero();
_accum.gyro.zero();
_accum.count = 0;
}
}
/*
@ -601,7 +610,7 @@ void AP_InertialSensor_MPU6000::_read_fifo()
}
while (n_samples > 0) {
uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES);
uint8_t n = MIN(n_samples, MPU6000_MAX_FIFO_SAMPLES - _accum.count);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU6000_SAMPLE_SIZE)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU6000_SAMPLE_SIZE);
goto check_registers;
@ -663,10 +672,10 @@ void AP_InertialSensor_MPU6000::_set_filter_register(void)
config = 0;
#endif
#if 0
// temporarily disable fast sampling
_fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
#endif
if (enable_fast_sampling(_accel_instance)) {
_fast_sampling = (_is_icm_device && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI);
}
if (_fast_sampling) {
// this gives us 8kHz sampling on gyros and 4kHz on accels

7
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.h
View File

@ -122,6 +122,13 @@ private:
uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
// accumulators for fast sampling
struct {
Vector3l accel;
Vector3l gyro;
uint8_t count;
} _accum;
};
class AP_MPU6000_AuxiliaryBusSlave : public AuxiliaryBusSlave

57
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -207,14 +207,11 @@ static const float GYRO_SCALE = 0.0174532f / 16.4f;
AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool fast_sampling,
enum Rotation rotation)
: AP_InertialSensor_Backend(imu)
, _temp_filter(1000, 1)
, _rotation(rotation)
, _dev(std::move(dev))
// fast sampling disabled for now
, _fast_sampling(false)
{
}
@ -231,7 +228,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
return nullptr;
}
AP_InertialSensor_MPU9250 *sensor =
new AP_InertialSensor_MPU9250(imu, std::move(dev), false, rotation);
new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
@ -244,7 +241,6 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
bool fast_sampling,
enum Rotation rotation)
{
if (!dev) {
@ -254,7 +250,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i
dev->set_read_flag(0x80);
sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), fast_sampling, rotation);
sensor = new AP_InertialSensor_MPU9250(imu, std::move(dev), rotation);
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
@ -313,6 +309,10 @@ void AP_InertialSensor_MPU9250::start()
// always use FIFO
_fifo_enable();
if (enable_fast_sampling(_accel_instance) && _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
_fast_sampling = true;
}
if (_fast_sampling) {
// setup for fast sampling
_register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2, true);
@ -458,10 +458,13 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
abs(a.z) > clip_limit) {
clipped = true;
}
asum += a;
gsum += Vector3l(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
Vector3l g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
_accum.accel += a;
_accum.gyro += g;
_accum.count++;
float temp = int16_val(data, 3);
temp = temp/340 + 36.53;
@ -472,20 +475,26 @@ void AP_InertialSensor_MPU9250::_accumulate_fast_sampling(uint8_t *samples, uint
if (clipped) {
increment_clip_count(_accel_instance);
}
float ascale = MPU9250_ACCEL_SCALE_1G / n_samples;
Vector3f accel(asum.x*ascale, asum.y*ascale, asum.z*ascale);
float gscale = GYRO_SCALE / n_samples;
Vector3f gyro(gsum.x*gscale, gsum.y*gscale, gsum.z*gscale);
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(tsum / n_samples);
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 gscale = GYRO_SCALE / _accum.count;
Vector3f gyro(_accum.gyro.x*gscale, _accum.gyro.y*gscale, _accum.gyro.z*gscale);
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, AP_HAL::micros64(), false);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_accum.accel.zero();
_accum.gyro.zero();
_accum.count = 0;
}
}
@ -535,7 +544,7 @@ bool AP_InertialSensor_MPU9250::_read_sample()
}
while (n_samples > 0) {
uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES, n_samples);
uint8_t n = MIN(MPU9250_MAX_FIFO_SAMPLES - _accum.count, n_samples);
if (!_block_read(MPUREG_FIFO_R_W, rx, n * MPU9250_SAMPLE_SIZE)) {
printf("MPU60x0: error in fifo read %u bytes\n", n * MPU9250_SAMPLE_SIZE);
goto check_registers;
@ -544,7 +553,7 @@ bool AP_InertialSensor_MPU9250::_read_sample()
if (_fast_sampling) {
_accumulate_fast_sampling(rx, n);
} else {
_accumulate_fast_sampling(rx, n);
_accumulate(rx, n);
}
n_samples -= n;
}

9
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -37,7 +37,6 @@ public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
bool fast_sampling = false,
enum Rotation rotation = ROTATION_NONE);
/* update accel and gyro state */
@ -53,7 +52,6 @@ public:
private:
AP_InertialSensor_MPU9250(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev,
bool fast_sampling,
enum Rotation rotation);
#if MPU9250_DEBUG
@ -112,6 +110,13 @@ private:
uint8_t *_fifo_buffer;
uint8_t _reg_check_counter;
// accumulators for fast sampling
struct {
Vector3l accel;
Vector3l gyro;
uint8_t count;
} _accum;
};
class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave