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

AP_InertialSensor: fixed threading and locking in remaining backends 

use a backend semaphore for shared data and use thread per bus where
available
This commit is contained in:
Andrew Tridgell 2016-11-04 11:06:19 +11:00
parent 2e4151d14e
commit a5c5e033d2
9 changed files with 57 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

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

45
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -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();
}

3
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

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

24
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

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

8
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -22,14 +22,16 @@ public:
AP_HAL::OwnPtr<AP_HAL::I2CDevice> 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<AP_HAL::I2CDevice> _dev;

2
libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp
View File

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

6
libraries/AP_InertialSensor/AP_InertialSensor_MPU6000.cpp
View File

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

35
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp
View File

@ -14,13 +14,12 @@
*/
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "AP_InertialSensor_MPU9250.h"
#include <assert.h>
#include <utility>
#include <AP_HAL_Linux/GPIO.h>
#include <AP_HAL/GPIO.h>
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

5
libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h
View File

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