Skip to content
Snippets Groups Projects
Commit dcdb5358 authored by Andrew Tridgell's avatar Andrew Tridgell
Browse files

AP_InertialSensor: use lockless structures in MPU9250 driver

this avoids suspending timers when transferring data between main
thread and SPI read thread
parent 7343de28
No related branches found
No related tags found
No related merge requests found
......@@ -177,6 +177,7 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
_initialised(false),
_mpu9250_product_id(AP_PRODUCT_ID_PIXHAWK_FIRE_CAPE),
_last_filter_hz(-1),
_shared_data_idx(0),
_accel_filter_x(1000, 15),
_accel_filter_y(1000, 15),
_accel_filter_z(1000, 15),
......@@ -197,6 +198,8 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
_spi = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore();
// we need to suspend timers to prevent other SPI drivers grabbing
// the bus while we do the long initialisation
hal.scheduler->suspend_timer_procs();
uint8_t whoami = _register_read(MPUREG_WHOAMI);
......@@ -232,7 +235,6 @@ uint16_t AP_InertialSensor_MPU9250::_init_sensor( Sample_rate sample_rate )
hal.scheduler->resume_timer_procs();
/* read the first lot of data.
* _read_data_transaction requires the spi semaphore to be taken by
* its caller. */
......@@ -298,11 +300,10 @@ bool AP_InertialSensor_MPU9250::update( void )
_previous_accel[0] = _accel[0];
// disable timer procs for mininum time
hal.scheduler->suspend_timer_procs();
_gyro[0] = _gyro_filtered;
_accel[0] = _accel_filtered;
hal.scheduler->resume_timer_procs();
// pull the data from the timer shared data buffer
uint8_t idx = _shared_data_idx;
_gyro[0] = _shared_data[idx]._gyro_filtered;
_accel[0] = _shared_data[idx]._accel_filtered;
_gyro[0].rotate(_board_orientation);
_gyro[0] *= GYRO_SCALE;
......@@ -374,13 +375,18 @@ void AP_InertialSensor_MPU9250::_read_data_transaction()
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
_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)));
Vector3f _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)));
Vector3f _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)));
// update the shared buffer
uint8_t idx = _shared_data_idx ^ 1;
_shared_data[idx]._accel_filtered = _accel_filtered;
_shared_data[idx]._gyro_filtered = _gyro_filtered;
_shared_data_idx = idx;
}
/*
......
......@@ -57,9 +57,15 @@ private:
// change the filter frequency
void _set_filter(uint8_t filter_hz);
// output of accel/gyro filters
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
// This structure is used to pass data from the timer which reads
// the sensor to the main thread. The _shared_data_idx is used to
// prevent race conditions by ensuring the data is fully updated
// before being used by the consumer
struct {
Vector3f _accel_filtered;
Vector3f _gyro_filtered;
} _shared_data[2];
volatile uint8_t _shared_data_idx;
// Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment