Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues 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

30
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -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)));
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)));
_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 _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;
}
/*

12
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

@ -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;