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

AP_InertialSensor: use lockless structures in MPU9250 driver

Browse Source 
this avoids suspending timers when transferring data between main
thread and SPI read thread
This commit is contained in:
Andrew Tridgell 2014-08-21 07:48:44 +10:00
parent 7343de2893
commit dcdb53584b
2 changed files with 27 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -177,6 +177,7 @@ AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250() :
_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), _last_filter_hz(-1),
_shared_data_idx(0),
_accel_filter_x(1000, 15), _accel_filter_x(1000, 15),
_accel_filter_y(1000, 15), _accel_filter_y(1000, 15),
_accel_filter_z(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 = hal.spi->device(AP_HAL::SPIDevice_MPU9250);
_spi_sem = _spi->get_semaphore(); _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(); hal.scheduler->suspend_timer_procs();
uint8_t whoami = _register_read(MPUREG_WHOAMI); 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(); hal.scheduler->resume_timer_procs();
/* 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. */
@ -298,11 +300,10 @@ bool AP_InertialSensor_MPU9250::update( void )
_previous_accel[0] = _accel[0]; _previous_accel[0] = _accel[0];
// disable timer procs for mininum time // pull the data from the timer shared data buffer
hal.scheduler->suspend_timer_procs(); uint8_t idx = _shared_data_idx;
_gyro[0] = _gyro_filtered; _gyro[0] = _shared_data[idx]._gyro_filtered;
_accel[0] = _accel_filtered; _accel[0] = _shared_data[idx]._accel_filtered;
hal.scheduler->resume_timer_procs();
_gyro[0].rotate(_board_orientation); _gyro[0].rotate(_board_orientation);
_gyro[0] *= GYRO_SCALE; _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])) #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)), Vector3f _accel_filtered = Vector3f(_accel_filter_x.apply(int16_val(rx.v, 1)),
_accel_filter_y.apply(int16_val(rx.v, 0)), _accel_filter_y.apply(int16_val(rx.v, 0)),
_accel_filter_z.apply(-int16_val(rx.v, 2))); _accel_filter_z.apply(-int16_val(rx.v, 2)));
_gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)), Vector3f _gyro_filtered = Vector3f(_gyro_filter_x.apply(int16_val(rx.v, 5)),
_gyro_filter_y.apply(int16_val(rx.v, 4)), _gyro_filter_y.apply(int16_val(rx.v, 4)),
_gyro_filter_z.apply(-int16_val(rx.v, 6))); _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;
} }
/* /*

8
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -57,9 +57,15 @@ private:
// change the filter frequency // change the filter frequency
void _set_filter(uint8_t filter_hz); void _set_filter(uint8_t filter_hz);
// output of accel/gyro filters // 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 _accel_filtered;
Vector3f _gyro_filtered; Vector3f _gyro_filtered;
} _shared_data[2];
volatile uint8_t _shared_data_idx;
// Low Pass filters for gyro and accel // Low Pass filters for gyro and accel
LowPassFilter2p _accel_filter_x; LowPassFilter2p _accel_filter_x;