5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00

AP_InertialSensor: use thread-per-bus for lsm303d and mpu6000

This commit is contained in:
Andrew Tridgell 2016-11-02 09:11:55 +11:00
parent ed6df09d12
commit 2b66df68a1
4 changed files with 13 additions and 41 deletions

View File

@ -15,7 +15,6 @@
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "AP_InertialSensor_LSM9DS0.h"
#include <utility>
@ -497,7 +496,7 @@ bool AP_InertialSensor_LSM9DS0::_hardware_init()
_set_accel_max_abs_offset(_accel_instance, 5.0f);
/* start the timer process to read samples */
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void));
_dev_gyro->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, bool));
return true;
@ -664,35 +663,15 @@ void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale)
/**
* Timer process to poll for new data from the LSM9DS0.
*/
void AP_InertialSensor_LSM9DS0::_poll_data()
bool AP_InertialSensor_LSM9DS0::_poll_data()
{
bool drdy_is_from_reg = _drdy_pin_num_a < 0 || _drdy_pin_num_g < 0;
bool gyro_ready;
bool accel_ready;
if (drdy_is_from_reg && !_spi_sem->take_nonblocking()) {
return;
if (_gyro_data_ready()) {
_read_data_transaction_g();
}
gyro_ready = _gyro_data_ready();
accel_ready = _accel_data_ready();
if (gyro_ready || accel_ready) {
if (!drdy_is_from_reg && !_spi_sem->take_nonblocking()) {
return;
}
if (gyro_ready) {
_read_data_transaction_g();
}
if (accel_ready) {
_read_data_transaction_a();
}
_spi_sem->give();
} else if(drdy_is_from_reg) {
_spi_sem->give();
if (_accel_data_ready()) {
_read_data_transaction_a();
}
return true;
}
bool AP_InertialSensor_LSM9DS0::_accel_data_ready()
@ -797,4 +776,3 @@ void AP_InertialSensor_LSM9DS0::_dump_registers(void)
}
#endif
#endif

View File

@ -47,7 +47,7 @@ private:
bool _accel_data_ready();
bool _gyro_data_ready();
void _poll_data();
bool _poll_data();
bool _init_sensor();
bool _hardware_init();

View File

@ -388,8 +388,7 @@ void AP_InertialSensor_MPU6000::start()
hal.scheduler->resume_timer_procs();
// start the timer process to read samples
hal.scheduler->register_timer_process(
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, void));
_dev->register_periodic_callback(1000, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU6000::_poll_data, bool));
}
/*
@ -434,21 +433,16 @@ bool AP_InertialSensor_MPU6000::_data_ready()
}
/*
* Timer process to poll for new data from the MPU6000.
* Timer process to poll for new data from the MPU6000. Called from bus thread with semaphore held
*/
void AP_InertialSensor_MPU6000::_poll_data()
bool AP_InertialSensor_MPU6000::_poll_data()
{
if (!_dev->get_semaphore()->take_nonblocking()) {
return;
}
if (_use_fifo) {
_read_fifo();
} else if (_data_ready()) {
_read_sample();
}
_dev->get_semaphore()->give();
return true;
}
void AP_InertialSensor_MPU6000::_accumulate(uint8_t *samples, uint8_t n_samples)

View File

@ -77,7 +77,7 @@ private:
bool _data_ready();
/* Poll for new data (non-blocking) */
void _poll_data();
bool _poll_data();
/* Read and write functions taking the differences between buses into
* account */