From a5c5e033d2fe2f4505006fad4a355f4a619b08ae Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 4 Nov 2016 11:06:19 +1100 Subject: [PATCH] AP_InertialSensor: fixed threading and locking in remaining backends use a backend semaphore for shared data and use thread per bus where available --- .../AP_InertialSensor/AP_InertialSensor.cpp | 2 +- .../AP_InertialSensor_Backend.cpp | 45 ++++++++++++------- .../AP_InertialSensor_Backend.h | 3 ++ .../AP_InertialSensor_L3G4200D.cpp | 24 +++++----- .../AP_InertialSensor_L3G4200D.h | 8 ++-- .../AP_InertialSensor_LSM9DS0.cpp | 2 - .../AP_InertialSensor_MPU6000.cpp | 6 --- .../AP_InertialSensor_MPU9250.cpp | 35 +++------------ .../AP_InertialSensor_MPU9250.h | 5 +-- 9 files changed, 57 insertions(+), 73 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 4ddafef903..c1a9bd45a0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -596,7 +596,7 @@ AP_InertialSensor::detect_backends(void) hal.spi->get_device(HAL_INS_LSM9DS0_G_NAME), hal.spi->get_device(HAL_INS_LSM9DS0_A_NAME), ROTATION_ROLL_180)); } else if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V4) { - _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME))); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); } // also add any PX4 backends (eg. canbus sensors) _add_backend(AP_InertialSensor_PX4::detect(*this)); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index 603229caa1..cfdaa9c4ca 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -9,7 +9,9 @@ const extern AP_HAL::HAL& hal; AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) : _imu(imu), _product_id(AP_PRODUCT_ID_NONE) -{} +{ + _sem = hal.util->new_semaphore(); +} void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vector3f &accel) { @@ -103,13 +105,15 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance, _imu._last_delta_angle[instance] = delta_angle; _imu._last_raw_gyro[instance] = gyro; - _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro); - if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) { - _imu._gyro_filter[instance].reset(); + if (_sem->take(0)) { + _imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro); + if (_imu._gyro_filtered[instance].is_nan() || _imu._gyro_filtered[instance].is_inf()) { + _imu._gyro_filter[instance].reset(); + } + _imu._new_gyro_data[instance] = true; + _sem->give(); } - _imu._new_gyro_data[instance] = true; - DataFlash_Class *dataflash = get_dataflash(); if (dataflash != nullptr) { uint64_t now = AP_HAL::micros64(); @@ -184,15 +188,18 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance, _imu._delta_velocity_acc[instance] += accel * dt; _imu._delta_velocity_acc_dt[instance] += dt; - _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel); - if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) { - _imu._accel_filter[instance].reset(); + if (_sem->take(0)) { + _imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel); + if (_imu._accel_filtered[instance].is_nan() || _imu._accel_filtered[instance].is_inf()) { + _imu._accel_filter[instance].reset(); + } + + _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]); + + _imu._new_accel_data[instance] = true; + _sem->give(); } - _imu.set_accel_peak_hold(instance, _imu._accel_filtered[instance]); - - _imu._new_accel_data[instance] = true; - DataFlash_Class *dataflash = get_dataflash(); if (dataflash != nullptr) { uint64_t now = AP_HAL::micros64(); @@ -251,7 +258,9 @@ void AP_InertialSensor_Backend::_publish_temperature(uint8_t instance, float tem */ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) { - hal.scheduler->suspend_timer_procs(); + if (!_sem->take_nonblocking()) { + return; + } if (_imu._new_gyro_data[instance]) { _publish_gyro(instance, _imu._gyro_filtered[instance]); @@ -264,7 +273,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) _last_gyro_filter_hz[instance] = _gyro_filter_cutoff(); } - hal.scheduler->resume_timer_procs(); + _sem->give(); } /* @@ -272,7 +281,9 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) */ void AP_InertialSensor_Backend::update_accel(uint8_t instance) { - hal.scheduler->suspend_timer_procs(); + if (!_sem->take_nonblocking()) { + return; + } if (_imu._new_accel_data[instance]) { _publish_accel(instance, _imu._accel_filtered[instance]); @@ -285,5 +296,5 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) _last_accel_filter_hz[instance] = _accel_filter_cutoff(); } - hal.scheduler->resume_timer_procs(); + _sem->give(); } diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 736c58a9a2..a82680a4a0 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -75,6 +75,9 @@ protected: // access to frontend AP_InertialSensor &_imu; + // semaphore for access to shared frontend data + AP_HAL::Semaphore *_sem; + void _rotate_and_correct_accel(uint8_t instance, Vector3f &accel); void _rotate_and_correct_gyro(uint8_t instance, Vector3f &gyro); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp index 798d61f42c..f2493d0d0a 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp @@ -194,16 +194,23 @@ bool AP_InertialSensor_L3G4200D::_init_sensor(void) L3G4200D_REG_FIFO_CTL_STREAM); hal.scheduler->delay(1); + _product_id = AP_PRODUCT_ID_L3G4200D; + _dev->get_semaphore()->give(); + return true; +} + +/* + startup the sensor + */ +void AP_InertialSensor_L3G4200D::start(void) +{ _gyro_instance = _imu.register_gyro(800); _accel_instance = _imu.register_accel(800); - _product_id = AP_PRODUCT_ID_L3G4200D; // start the timer process to read samples - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, void)); - - return true; + _dev->register_periodic_callback(1250, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_L3G4200D::_accumulate, bool)); } /* @@ -218,12 +225,8 @@ bool AP_InertialSensor_L3G4200D::update(void) } // Accumulate values from accels and gyros -void AP_InertialSensor_L3G4200D::_accumulate(void) +bool AP_InertialSensor_L3G4200D::_accumulate(void) { - if (!_dev->get_semaphore()->take_nonblocking()) { - return; - } - uint8_t num_samples_available; uint8_t fifo_status = 0; @@ -276,9 +279,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void) } } } - - // give back i2c semaphore - _dev->get_semaphore()->give(); } #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h index fe04bdcf7b..c976ed2935 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h @@ -22,14 +22,16 @@ public: AP_HAL::OwnPtr dev); /* update accel and gyro state */ - bool update(); + bool update() override; // return product ID - int16_t product_id() const { return AP_PRODUCT_ID_L3G4200D; } + int16_t product_id() const override { return AP_PRODUCT_ID_L3G4200D; } + + void start(void) override; private: bool _init_sensor(); - void _accumulate(); + bool _accumulate(); AP_HAL::OwnPtr _dev; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp index 0bbc9f182b..d6a5839e60 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -436,9 +436,7 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor() _drdy_pin_g->mode(HAL_GPIO_INPUT); } - hal.scheduler->suspend_timer_procs(); bool success = _hardware_init(); - hal.scheduler->resume_timer_procs(); #if LSM9DS0_DEBUG _dump_registers(); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp index b21948c78e..2b89a0be3d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp @@ -288,9 +288,7 @@ bool AP_InertialSensor_MPU6000::_init() _drdy_pin->mode(HAL_GPIO_INPUT); #endif - hal.scheduler->suspend_timer_procs(); bool success = _hardware_init(); - hal.scheduler->resume_timer_procs(); #if MPU6000_DEBUG _dump_registers(); @@ -321,8 +319,6 @@ bool AP_InertialSensor_MPU6000::_has_auxiliary_bus() void AP_InertialSensor_MPU6000::start() { - hal.scheduler->suspend_timer_procs(); - if (!_dev->get_semaphore()->take(100)) { AP_HAL::panic("MPU6000: Unable to get semaphore"); } @@ -393,8 +389,6 @@ void AP_InertialSensor_MPU6000::start() set_gyro_orientation(_gyro_instance, _rotation); set_accel_orientation(_accel_instance, _rotation); - hal.scheduler->resume_timer_procs(); - // start the timer process to read samples if (_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool)) == nullptr) { // cope with AuxiliaryBus which does not support register_periodic_callback yet diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index e8332b0cc6..5c3347cb08 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -14,13 +14,12 @@ */ #include -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include "AP_InertialSensor_MPU9250.h" #include #include -#include +#include extern const AP_HAL::HAL &hal; @@ -264,9 +263,7 @@ AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &i bool AP_InertialSensor_MPU9250::_init() { - hal.scheduler->suspend_timer_procs(); bool success = _hardware_init(); - hal.scheduler->resume_timer_procs(); #if MPU9250_DEBUG _dump_registers(); @@ -282,8 +279,6 @@ bool AP_InertialSensor_MPU9250::_has_auxiliary_bus() void AP_InertialSensor_MPU9250::start() { - hal.scheduler->suspend_timer_procs(); - if (!_dev->get_semaphore()->take(100)) { AP_HAL::panic("MPU92500: Unable to get semaphore"); } @@ -330,11 +325,8 @@ void AP_InertialSensor_MPU9250::start() _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE); _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE); - hal.scheduler->resume_timer_procs(); - // start the timer process to read samples - hal.scheduler->register_timer_process( - FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void)); + _dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_read_sample, bool)); } /* @@ -375,20 +367,6 @@ bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status) return (int_status & BIT_RAW_RDY_INT) != 0; } -/* - * Timer process to poll for new data from the MPU9250. - */ -void AP_InertialSensor_MPU9250::_poll_data() -{ - if (!_dev->get_semaphore()->take_nonblocking()) { - return; - } - - _read_sample(); - - _dev->get_semaphore()->give(); -} - void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx) { Vector3f accel, gyro; @@ -414,7 +392,7 @@ void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx) /* * read from the data registers and update filtered data */ -void AP_InertialSensor_MPU9250::_read_sample() +bool AP_InertialSensor_MPU9250::_read_sample() { /* one register address followed by seven 2-byte registers */ struct PACKED { @@ -424,14 +402,15 @@ void AP_InertialSensor_MPU9250::_read_sample() if (!_block_read(MPUREG_INT_STATUS, (uint8_t *) &rx, sizeof(rx))) { hal.console->printf("MPU9250: error reading sample\n"); - return; + return true; } if (!_data_ready(rx.int_status)) { - return; + return true; } _accumulate(rx.d); + return true; } bool AP_InertialSensor_MPU9250::_block_read(uint8_t reg, uint8_t *buf, @@ -700,4 +679,4 @@ int AP_MPU9250_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave, return 0; } -#endif + diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index 61ddaa8ab0..c96945113d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -62,15 +62,12 @@ private: bool _has_auxiliary_bus(); /* Read a single sample */ - void _read_sample(); + bool _read_sample(); /* Check if there's data available by reading register */ bool _data_ready(); bool _data_ready(uint8_t int_status); - /* Poll for new data (non-blocking) */ - void _poll_data(); - /* Read and write functions taking the differences between buses into * account */ bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size);