AP_InertialSensor: use runtime GPIO drdy pins numbers instead C constant macros

As we intend to eventually get board related parameters from a configuration
file, this commit makes the GPIO numbers for data-ready pins be instance
variables instead of from C constant macros.

Another advantage of using instance variables in this context is the
possibility of using more than one LSM9DS0.
This commit is contained in:
Gustavo Jose de Sousa 2015-06-29 10:38:06 -03:00 committed by Andrew Tridgell
parent 0ef914adac
commit 2f2a627002
2 changed files with 63 additions and 83 deletions

View File

@ -27,18 +27,6 @@ extern const AP_HAL::HAL& hal;
#define LSM9DS0_G_WHOAMI 0xD4 #define LSM9DS0_G_WHOAMI 0xD4
#define LSM9DS0_XM_WHOAMI 0x49 #define LSM9DS0_XM_WHOAMI 0x49
/*
* If data-ready GPIO pins are not defined, the fallback approach used is to
* check if there's new data ready by reading the status register. It is
* *strongly* recommended to use data-ready GPIO pins for performance reasons.
*/
#ifndef DRDY_GPIO_PIN_A
#define DRDY_GPIO_PIN_A 0
#endif
#ifndef DRDY_GPIO_PIN_G
#define DRDY_GPIO_PIN_G 0
#endif
//////////////////////////// ////////////////////////////
// LSM9DS0 Gyro Registers // // LSM9DS0 Gyro Registers //
//////////////////////////// ////////////////////////////
@ -382,7 +370,9 @@ extern const AP_HAL::HAL& hal;
#define ACT_THS 0x3E #define ACT_THS 0x3E
#define ACT_DUR 0x3F #define ACT_DUR 0x3F
AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu): AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
int drdy_pin_num_a,
int drdy_pin_num_g):
AP_InertialSensor_Backend(imu), AP_InertialSensor_Backend(imu),
_drdy_pin_a(NULL), _drdy_pin_a(NULL),
_drdy_pin_g(NULL), _drdy_pin_g(NULL),
@ -391,14 +381,19 @@ AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu):
_accel_filter(800, 15), _accel_filter(800, 15),
_gyro_filter(760, 15), _gyro_filter(760, 15),
_gyro_sample_available(false), _gyro_sample_available(false),
_accel_sample_available(false) _accel_sample_available(false),
_drdy_pin_num_a(drdy_pin_num_a),
_drdy_pin_num_g(drdy_pin_num_g)
{ {
_product_id = AP_PRODUCT_ID_NONE; _product_id = AP_PRODUCT_ID_NONE;
} }
AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::detect(AP_InertialSensor &_imu) AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::detect(AP_InertialSensor &_imu)
{ {
AP_InertialSensor_LSM9DS0 *sensor = new AP_InertialSensor_LSM9DS0(_imu); int drdy_pin_num_a = -1, drdy_pin_num_g = -1;
AP_InertialSensor_LSM9DS0 *sensor =
new AP_InertialSensor_LSM9DS0(_imu, drdy_pin_num_a, drdy_pin_num_g);
if (sensor == NULL) { if (sensor == NULL) {
return NULL; return NULL;
@ -418,19 +413,19 @@ bool AP_InertialSensor_LSM9DS0::_init_sensor()
_accel_spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_AM); _accel_spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_AM);
_spi_sem = _gyro_spi->get_semaphore(); /* same semaphore for both */ _spi_sem = _gyro_spi->get_semaphore(); /* same semaphore for both */
#if DRDY_GPIO_PIN_A != 0 if (_drdy_pin_num_a >= 0) {
_drdy_pin_a = hal.gpio->channel(DRDY_GPIO_PIN_A); _drdy_pin_a = hal.gpio->channel(_drdy_pin_num_a);
if (_drdy_pin_a == NULL) { if (_drdy_pin_a == NULL) {
hal.scheduler->panic("LSM9DS0: null accel data-ready GPIO channel\n"); hal.scheduler->panic("LSM9DS0: null accel data-ready GPIO channel\n");
} }
#endif }
#if DRDY_GPIO_PIN_G != 0 if (_drdy_pin_num_g >= 0) {
_drdy_pin_g = hal.gpio->channel(DRDY_GPIO_PIN_G); _drdy_pin_g = hal.gpio->channel(_drdy_pin_num_g);
if (_drdy_pin_g == NULL) { if (_drdy_pin_g == NULL) {
hal.scheduler->panic("LSM9DS0: null gyro data-ready GPIO channel\n"); hal.scheduler->panic("LSM9DS0: null gyro data-ready GPIO channel\n");
} }
#endif }
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
@ -659,17 +654,13 @@ void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale)
/** /**
* Timer process to poll for new data from the LSM9DS0. * Timer process to poll for new data from the LSM9DS0.
*/ */
#if DRDY_GPIO_PIN_A != 0 && DRDY_GPIO_PIN_G != 0
void AP_InertialSensor_LSM9DS0::_poll_data() void AP_InertialSensor_LSM9DS0::_poll_data()
{ {
bool gyro_ready = _gyro_data_ready(); bool drdy_is_from_reg = _drdy_pin_num_a < 0 || _drdy_pin_num_g < 0;
bool accel_ready = _accel_data_ready(); bool gyro_ready;
bool accel_ready;
if (!gyro_ready && !accel_ready) { if (drdy_is_from_reg && !_spi_sem->take_nonblocking()) {
return;
}
if (!_spi_sem->take_nonblocking()) {
/* /*
* the semaphore being busy is an expected condition when the * the semaphore being busy is an expected condition when the
* mainline code is calling wait_for_sample() which will * mainline code is calling wait_for_sample() which will
@ -679,66 +670,46 @@ void AP_InertialSensor_LSM9DS0::_poll_data()
return; return;
} }
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) { if (gyro_ready) {
_read_data_transaction_g(); _read_data_transaction_g();
} }
if (accel_ready) { if (accel_ready) {
_read_data_transaction_a(); _read_data_transaction_a();
} }
_spi_sem->give(); _spi_sem->give();
} } else if(drdy_is_from_reg) {
#else
void AP_InertialSensor_LSM9DS0::_poll_data()
{
if (!_spi_sem->take_nonblocking()) {
/*
* the semaphore being busy is an expected condition when the
* mainline code is calling wait_for_sample() which will
* grab the semaphore. We return now and rely on the mainline
* code grabbing the latest sample.
*/
return;
}
if (_gyro_data_ready()) {
_read_data_transaction_g();
}
if (_accel_data_ready()) {
_read_data_transaction_a();
}
_spi_sem->give(); _spi_sem->give();
} }
#endif /* DRDY_GPIO_PIN_A != 0 && DRDY_GPIO_PIN_G != 0 */
#if DRDY_GPIO_PIN_A != 0
bool AP_InertialSensor_LSM9DS0::_accel_data_ready()
{
return _drdy_pin_a->read() != 0;
} }
#else
bool AP_InertialSensor_LSM9DS0::_accel_data_ready() bool AP_InertialSensor_LSM9DS0::_accel_data_ready()
{ {
if (_drdy_pin_a != NULL) {
return _drdy_pin_a->read() != 0;
} else {
uint8_t status = _register_read_xm(STATUS_REG_A); uint8_t status = _register_read_xm(STATUS_REG_A);
return status & STATUS_REG_A_ZYXADA; return status & STATUS_REG_A_ZYXADA;
} }
#endif /* DRDY_GPIO_PIN_A != 0 */
#if DRDY_GPIO_PIN_G != 0
bool AP_InertialSensor_LSM9DS0::_gyro_data_ready()
{
return _drdy_pin_g->read() != 0;
} }
#else
bool AP_InertialSensor_LSM9DS0::_gyro_data_ready() bool AP_InertialSensor_LSM9DS0::_gyro_data_ready()
{ {
if (_drdy_pin_g != NULL) {
return _drdy_pin_g->read() != 0;
} else {
uint8_t status = _register_read_xm(STATUS_REG_G); uint8_t status = _register_read_xm(STATUS_REG_G);
return status & STATUS_REG_G_ZYXDA; return status & STATUS_REG_G_ZYXDA;
} }
#endif /* DRDY_GPIO_PIN_G != 0 */ }
void AP_InertialSensor_LSM9DS0::_accel_raw_data(struct sensor_raw_data *raw_data) void AP_InertialSensor_LSM9DS0::_accel_raw_data(struct sensor_raw_data *raw_data)
{ {

View File

@ -27,7 +27,8 @@ public:
A_SCALE_16G A_SCALE_16G
}; };
AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu); AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu,
int drdy_pin_num_a, int drdy_pin_num_b);
bool update(); bool update();
@ -48,7 +49,15 @@ private:
AP_HAL::SPIDeviceDriver *_gyro_spi; AP_HAL::SPIDeviceDriver *_gyro_spi;
AP_HAL::Semaphore *_spi_sem; AP_HAL::Semaphore *_spi_sem;
/*
* If data-ready GPIO pins numbers are not defined (i.e. any negative
* value), the fallback approach used is to check if there's new data ready
* by reading the status register. It is *strongly* recommended to use
* data-ready GPIO pins for performance reasons.
*/
int _drdy_pin_num_a;
AP_HAL::DigitalSource *_drdy_pin_a; AP_HAL::DigitalSource *_drdy_pin_a;
int _drdy_pin_num_g;
AP_HAL::DigitalSource *_drdy_pin_g; AP_HAL::DigitalSource *_drdy_pin_g;
bool _gyro_sample_available; bool _gyro_sample_available;