From 02a7fa5c2bd0f3712f6c44576babe86edb07fb2c Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 20 Jan 2016 20:20:35 -0200 Subject: [PATCH] AP_InertialSensor: MPU9250: use AP_HAL::Device abstraction This makes MPU9250 be almost the same as MPU6000 driver. Work has been done here to make than similar so it's easier to spot the differences. --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 2 +- libraries/AP_HAL/AP_HAL_Boards.h | 17 +- .../AP_InertialSensor/AP_InertialSensor.cpp | 10 +- .../AP_InertialSensor_MPU9250.cpp | 671 ++++++++---------- .../AP_InertialSensor_MPU9250.h | 181 +++-- 5 files changed, 384 insertions(+), 497 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 3fa15ec6fc..0dc9e25e1d 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -361,7 +361,7 @@ AP_AK8963_SerialBus_MPU9250::AP_AK8963_SerialBus_MPU9250(AP_InertialSensor &ins, { // Only initialize members. Fails are handled by configure or while // getting the semaphore - _bus = ins.get_auxiliary_bus(HAL_INS_MPU9250, mpu9250_instance); + _bus = ins.get_auxiliary_bus(HAL_INS_MPU9250_SPI, mpu9250_instance); if (!_bus) AP_HAL::panic("Cannot get MPU9250 auxiliary bus"); diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index fa112aeb9a..13d1cbdaba 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -61,7 +61,7 @@ #define HAL_INS_FLYMAPLE 6 #define HAL_INS_L3G4200D 7 #define HAL_INS_VRBRAIN 8 -#define HAL_INS_MPU9250 9 +#define HAL_INS_MPU9250_SPI 9 #define HAL_INS_L3GD20 10 #define HAL_INS_LSM9DS0 11 #define HAL_INS_RASPILOT 12 @@ -200,7 +200,8 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" -#define HAL_INS_DEFAULT HAL_INS_MPU9250 +#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI +#define HAL_INS_MPU9250_NAME "mpu9250" #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI #define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 @@ -274,7 +275,8 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" -#define HAL_INS_DEFAULT HAL_INS_MPU9250 +#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI +#define HAL_INS_MPU9250_NAME "mpu9250" #define HAL_BARO_DEFAULT HAL_BARO_MS5611_I2C #define HAL_BARO_MS5611_I2C_BUS 1 #define HAL_BARO_MS5611_I2C_ADDR 0x77 @@ -296,7 +298,8 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" -#define HAL_INS_DEFAULT HAL_INS_MPU9250 +#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI +#define HAL_INS_MPU9250_NAME "mpu9250" #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI #define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 @@ -315,7 +318,8 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" -#define HAL_INS_DEFAULT HAL_INS_MPU9250 +#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI +#define HAL_INS_MPU9250_NAME "mpu9250" #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI #define HAL_BARO_MS5611_NAME "ms5611" #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 @@ -368,7 +372,8 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI #define HAL_BOARD_LOG_DIRECTORY "/var/APM/logs" #define HAL_BOARD_TERRAIN_DIRECTORY "/var/APM/terrain" -#define HAL_INS_DEFAULT HAL_INS_MPU9250 +#define HAL_INS_DEFAULT HAL_INS_MPU9250_SPI +#define HAL_INS_MPU9250_NAME "mpu9250" #define HAL_BARO_DEFAULT HAL_BARO_MS5611_SPI #define HAL_COMPASS_DEFAULT HAL_COMPASS_AK8963_MPU9250 #define HAL_GPIO_A_LED_PIN 24 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 1f03efec61..53dd239be7 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -526,11 +526,11 @@ AP_InertialSensor::detect_backends(void) _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR))); #elif HAL_INS_DEFAULT == HAL_INS_BH _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU60x0_I2C_BUS, HAL_INS_MPU60x0_I2C_ADDR))); - _add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250))); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_PX4 || HAL_INS_DEFAULT == HAL_INS_VRBRAIN _add_backend(AP_InertialSensor_PX4::detect(*this)); -#elif HAL_INS_DEFAULT == HAL_INS_MPU9250 - _add_backend(AP_InertialSensor_MPU9250::detect(*this, hal.spi->device(AP_HAL::SPIDevice_MPU9250))); +#elif HAL_INS_DEFAULT == HAL_INS_MPU9250_SPI + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE _add_backend(AP_InertialSensor_Flymaple::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0 @@ -544,9 +544,7 @@ AP_InertialSensor::detect_backends(void) //_add_backend(AP_InertialSensor_LSM303D::detect); _add_backend(AP_InertialSensor_MPU6000::probe(*this, hal.spi->get_device(HAL_INS_MPU60x0_NAME))); #elif HAL_INS_DEFAULT == HAL_INS_MPU9250_I2C - _add_backend(AP_InertialSensor_MPU9250::detect_i2c(*this, - HAL_INS_MPU9250_I2C_POINTER, - HAL_INS_MPU9250_I2C_ADDR)); + _add_backend(AP_InertialSensor_MPU9250::probe(*this, hal.i2c_mgr->get_device(HAL_INS_MPU9250_I2C_BUS, HAL_INS_MPU9250_I2C_ADDR)); #elif HAL_INS_DEFAULT == HAL_INS_QFLIGHT _add_backend(AP_InertialSensor_QFLIGHT::detect(*this)); #elif HAL_INS_DEFAULT == HAL_INS_QURT diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp index 30360ecdf3..3606dd870f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.cpp @@ -12,19 +12,18 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . - - -- Coded by Victor Mayoral Vilches -- */ - #include -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include "AP_InertialSensor_MPU9250.h" -#include #include +#include -extern const AP_HAL::HAL& hal; +#include + +extern const AP_HAL::HAL &hal; // MPU9250 accelerometer scaling for 16g range #define MPU9250_ACCEL_SCALE_1G (GRAVITY_MSS / 2048.0f) @@ -181,11 +180,18 @@ extern const AP_HAL::HAL& hal; #define DEFAULT_SMPLRT_DIV MPUREG_SMPLRT_1000HZ #define DEFAULT_SAMPLE_RATE (1000 / (DEFAULT_SMPLRT_DIV + 1)) +#define MPU9250_SAMPLE_SIZE 14 +#define MPU9250_MAX_FIFO_SAMPLES 3 +#define MAX_DATA_READ (MPU9250_MAX_FIFO_SAMPLES * MPU9250_SAMPLE_SIZE) + +#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) +#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]) + /* * PS-MPU-9250A-00.pdf, page 8, lists LSB sensitivity of * gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3) */ -#define GYRO_SCALE (0.0174532f / 16.4f) +static const float GYRO_SCALE = 0.0174532f / 16.4f; /* * PS-MPU-9250A-00.pdf, page 9, lists LSB sensitivity of @@ -195,263 +201,145 @@ extern const AP_HAL::HAL& hal; * variants however */ -/* - * 2 bytes for each in this order: ACC_X, ACC_Y, ACC_Z, TEMP, GYRO_X, GYRO_Y - * and GYRO_Z - */ -#define MPU9250_SAMPLE_SIZE 14 - -/* SPI bus driver implementation */ -AP_MPU9250_BusDriver_SPI::AP_MPU9250_BusDriver_SPI(AP_HAL::SPIDeviceDriver *spi) -{ - _spi = spi; -} - -void AP_MPU9250_BusDriver_SPI::init() -{ - // disable I2C as recommended by the datasheet - write8(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); -} - -void AP_MPU9250_BusDriver_SPI::read8(uint8_t reg, uint8_t *val) -{ - uint8_t addr = reg | 0x80; // Set most significant bit - - uint8_t tx[2]; - uint8_t rx[2]; - - tx[0] = addr; - tx[1] = 0; - _spi->transaction(tx, rx, 2); - - *val = rx[1]; -} - -void AP_MPU9250_BusDriver_SPI::read_block(uint8_t reg, uint8_t *val, uint8_t count) -{ - assert(count < 32); - - uint8_t addr = reg | 0x80; // Set most significant bit - uint8_t tx[32] = { addr, }; - uint8_t rx[32]; - - _spi->transaction(tx, rx, count + 1); - memcpy(val, rx + 1, count); -} - -void AP_MPU9250_BusDriver_SPI::write8(uint8_t reg, uint8_t val) -{ - uint8_t tx[2]; - uint8_t rx[2]; - - tx[0] = reg; - tx[1] = val; - _spi->transaction(tx, rx, 2); -} - -void AP_MPU9250_BusDriver_SPI::set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) -{ - _spi->set_bus_speed(speed); -} - -bool AP_MPU9250_BusDriver_SPI::read_data_transaction(uint8_t *samples, uint8_t &n_samples) -{ - /* one register address followed by seven 2-byte registers */ - struct PACKED { - uint8_t cmd; - uint8_t int_status; - uint8_t v[MPU9250_SAMPLE_SIZE]; - } rx, tx = { cmd : MPUREG_INT_STATUS | 0x80, }; - - _spi->transaction((const uint8_t *)&tx, (uint8_t *)&rx, sizeof(rx)); - - if (!(rx.int_status & BIT_RAW_RDY_INT)) { - n_samples = 0; -#if MPU9250_DEBUG - hal.console->printf("MPU9250: No sample available.\n"); -#endif - return false; - } - - n_samples = 1; - memcpy(&samples[0], &rx.v[0], MPU9250_SAMPLE_SIZE); - - return true; -} - -AP_HAL::Semaphore* AP_MPU9250_BusDriver_SPI::get_semaphore() -{ - return _spi->get_semaphore(); -} - -bool AP_MPU9250_BusDriver_SPI::has_auxiliary_bus() -{ - return true; -} - -/* I2C bus driver implementation */ -AP_MPU9250_BusDriver_I2C::AP_MPU9250_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr) - : _addr(addr) - , _i2c(i2c) -{ -} - -void AP_MPU9250_BusDriver_I2C::init() -{ - uint8_t value; - - read8(MPUREG_INT_PIN_CFG, &value); - // enable I2C bypass, connecting auxiliary I2C bus to the main one - value |= BIT_BYPASS_EN; - write8(MPUREG_INT_PIN_CFG, value); -} - -void AP_MPU9250_BusDriver_I2C::read8(uint8_t reg, uint8_t *val) -{ - _i2c->readRegister(_addr, reg, val); -} - -void AP_MPU9250_BusDriver_I2C::read_block(uint8_t reg, uint8_t *val, uint8_t count) -{ - _i2c->readRegisters(_addr, reg, count, val); -} - -void AP_MPU9250_BusDriver_I2C::write8(uint8_t reg, uint8_t val) -{ - _i2c->writeRegister(_addr, reg, val); -} - -bool AP_MPU9250_BusDriver_I2C::read_data_transaction(uint8_t *samples, - uint8_t &n_samples) -{ - uint8_t ret = 0; - struct PACKED { - uint8_t int_status; - uint8_t v[MPU9250_SAMPLE_SIZE]; - } buffer; - - ret = _i2c->readRegisters(_addr, MPUREG_INT_STATUS, sizeof(buffer), (uint8_t *)&buffer); - if (ret != 0) { - hal.console->printf("MPU9250: error in I2C read\n"); - n_samples = 0; - return false; - } - - if (!(buffer.int_status & BIT_RAW_RDY_INT)) { -#if MPU9250_DEBUG - hal.console->printf("MPU9250: No sample available.\n"); -#endif - n_samples = 0; - return false; - } - - memcpy(samples, buffer.v, MPU9250_SAMPLE_SIZE); - n_samples = 1; - return true; -} - -AP_HAL::Semaphore* AP_MPU9250_BusDriver_I2C::get_semaphore() -{ - return _i2c->get_semaphore(); -} - -bool AP_MPU9250_BusDriver_I2C::has_auxiliary_bus() -{ - return false; -} - -AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus) : - AP_InertialSensor_Backend(imu), - _bus(bus), +AP_InertialSensor_MPU9250::AP_InertialSensor_MPU9250(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev, + enum bus_type type, + uint8_t read_flag) + : AP_InertialSensor_Backend(imu) + , _read_flag(read_flag) + , _bus_type(type) #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF - _default_rotation(ROTATION_ROLL_180_YAW_270) + , _default_rotation(ROTATION_ROLL_180_YAW_270) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO - /* no rotation needed */ - _default_rotation(ROTATION_NONE) + , _default_rotation(ROTATION_NONE) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI - _default_rotation(ROTATION_YAW_270) + , _default_rotation(ROTATION_YAW_270) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - _default_rotation(ROTATION_NONE) + , _default_rotation(ROTATION_NONE) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH - _default_rotation(ROTATION_NONE) -#else /* rotate for bbone default (and other boards) */ - _default_rotation(ROTATION_ROLL_180_YAW_90) + , _default_rotation(ROTATION_NONE) +#else + /* rotate for PXF (and default for other boards) */ + , _default_rotation(ROTATION_ROLL_180_YAW_90) #endif + , _dev(std::move(dev)) { } -/* - detect the sensor - */ -AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect(AP_InertialSensor &_imu, AP_HAL::SPIDeviceDriver *spi) +AP_InertialSensor_MPU9250::~AP_InertialSensor_MPU9250() { - AP_MPU9250_BusDriver *bus = new AP_MPU9250_BusDriver_SPI(spi); - if (!bus) - return NULL; - return _detect(_imu, bus, HAL_INS_MPU9250); + delete _auxiliary_bus; } -AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::detect_i2c(AP_InertialSensor &_imu, - AP_HAL::I2CDriver *i2c, - uint8_t addr) +AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev) { - AP_MPU9250_BusDriver *bus = new AP_MPU9250_BusDriver_I2C(i2c, addr); - if (!bus) - return nullptr; - return _detect(_imu, bus, HAL_INS_MPU9250); -} - -/* Common detection method - it takes ownership of the bus, freeing it if it's - * not possible to return an AP_InertialSensor_Backend */ -AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::_detect(AP_InertialSensor &_imu, - AP_MPU9250_BusDriver *bus, - int16_t id) -{ - AP_InertialSensor_MPU9250 *sensor = new AP_InertialSensor_MPU9250(_imu, bus); - if (sensor == NULL) { - delete bus; - return NULL; - } - if (!sensor->_init_sensor()) { + AP_InertialSensor_MPU9250 *sensor = + new AP_InertialSensor_MPU9250(imu, std::move(dev), BUS_TYPE_I2C, 0); + if (!sensor || !sensor->_init()) { delete sensor; - delete bus; - return NULL; + return nullptr; } - - sensor->_id = id; + sensor->_id = HAL_INS_MPU9250_I2C; return sensor; } -/* - initialise the sensor - */ -bool AP_InertialSensor_MPU9250::_init_sensor() + +AP_InertialSensor_Backend *AP_InertialSensor_MPU9250::probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev) { - _bus_sem = _bus->get_semaphore(); + AP_InertialSensor_MPU9250 *sensor = + new AP_InertialSensor_MPU9250(imu, std::move(dev), BUS_TYPE_SPI, 0x80); + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + sensor->_id = HAL_INS_MPU9250_SPI; - if (!_hardware_init()) - return false; + return sensor; +} - _gyro_instance = _imu.register_gyro(DEFAULT_SAMPLE_RATE); - _accel_instance = _imu.register_accel(DEFAULT_SAMPLE_RATE); - - _product_id = AP_PRODUCT_ID_MPU9250; - - // start the timer process to read samples - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_MPU9250::_poll_data, void)); +bool AP_InertialSensor_MPU9250::_init() +{ + hal.scheduler->suspend_timer_procs(); + bool success = _hardware_init(); + hal.scheduler->resume_timer_procs(); #if MPU9250_DEBUG _dump_registers(); #endif - return true; + + return success; +} + +bool AP_InertialSensor_MPU9250::_has_auxiliary_bus() +{ + return _bus_type != BUS_TYPE_I2C; +} + +void AP_InertialSensor_MPU9250::start() +{ + hal.scheduler->suspend_timer_procs(); + + if (!_dev->get_semaphore()->take(100)) { + AP_HAL::panic("MPU92500: Unable to get semaphore"); + } + + // initially run the bus at low speed + _dev->set_speed(AP_HAL::Device::SPEED_LOW); + + // only used for wake-up in accelerometer only low power mode + _register_write(MPUREG_PWR_MGMT_2, 0x00); + hal.scheduler->delay(1); + + // disable sensor filtering + _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2); + + // set sample rate to 1kHz, and use the 2 pole filter to give the + // desired rate + _register_write(MPUREG_SMPLRT_DIV, DEFAULT_SMPLRT_DIV); + hal.scheduler->delay(1); + + // Gyro scale 2000º/s + _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); + hal.scheduler->delay(1); + + _product_id = AP_PRODUCT_ID_MPU9250; + + // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g + _register_write(MPUREG_ACCEL_CONFIG,3<<3); + + // configure interrupt to fire when new data arrives + _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); + + // clear interrupt on any read, and hold the data ready pin high + // until we clear the interrupt + uint8_t value = _register_read(MPUREG_INT_PIN_CFG); + value |= BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN; + _register_write(MPUREG_INT_PIN_CFG, value); + + // now that we have initialised, we set the bus speed to high + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + _dev->get_semaphore()->give(); + + // grab the used instances + _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)); } /* update the accel and gyro vectors */ -bool AP_InertialSensor_MPU9250::update( void ) +bool AP_InertialSensor_MPU9250::update() { update_gyro(_gyro_instance); update_accel(_accel_instance); @@ -459,41 +347,50 @@ bool AP_InertialSensor_MPU9250::update( void ) return true; } -/*================ HARDWARE FUNCTIONS ==================== */ - -/** - * Timer process to poll for new data from the MPU9250. - */ -void AP_InertialSensor_MPU9250::_poll_data(void) +AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus() { - if (!_bus_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 (_auxiliary_bus) { + return _auxiliary_bus; } - _read_data_transaction(); - _bus_sem->give(); + + if (_has_auxiliary_bus()) { + _auxiliary_bus = new AP_MPU9250_AuxiliaryBus(*this); + } + + return _auxiliary_bus; } /* - read from the data registers and update filtered data + * Return true if the MPU9250 has new data available for reading. */ -void AP_InertialSensor_MPU9250::_read_data_transaction() +bool AP_InertialSensor_MPU9250::_data_ready() { - uint8_t n_samples; - uint8_t rx[MPU9250_SAMPLE_SIZE]; + uint8_t int_status = _register_read(MPUREG_INT_STATUS); + return _data_ready(int_status); +} - Vector3f accel, gyro; +bool AP_InertialSensor_MPU9250::_data_ready(uint8_t int_status) +{ + return (int_status & BIT_RAW_RDY_INT) != 0; +} - if (!_bus->read_data_transaction(rx, n_samples)) { +/* + * Timer process to poll for new data from the MPU6000. + */ +void AP_InertialSensor_MPU9250::_poll_data() +{ + if (!_dev->get_semaphore()->take_nonblocking()) { return; } -#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])) + _read_sample(); + + _dev->get_semaphore()->give(); +} + +void AP_InertialSensor_MPU9250::_accumulate(uint8_t *rx) +{ + Vector3f accel, gyro; accel = Vector3f(int16_val(rx, 1), int16_val(rx, 0), @@ -508,45 +405,80 @@ void AP_InertialSensor_MPU9250::_read_data_transaction() -int16_val(rx, 6)); gyro *= GYRO_SCALE; gyro.rotate(_default_rotation); - _rotate_and_correct_gyro(_gyro_instance, gyro); _notify_new_gyro_raw_sample(_gyro_instance, gyro); } + /* - read an 8 bit register + * read from the data registers and update filtered data */ +void AP_InertialSensor_MPU9250::_read_sample() +{ + /* one register address followed by seven 2-byte registers */ + struct PACKED { + uint8_t int_status; + uint8_t d[14]; + } rx; + + if (!_block_read(MPUREG_INT_STATUS, (uint8_t *) &rx, sizeof(rx))) { + hal.console->printf("MPU9250: error reading sample\n"); + return; + } + + if (!_data_ready(rx.int_status)) { + return; + } + + _accumulate(rx.d); +} + +bool AP_InertialSensor_MPU9250::_block_read(uint8_t reg, uint8_t *buf, + uint32_t size) +{ + reg |= _read_flag; + return _dev->read_registers(reg, buf, size); +} + uint8_t AP_InertialSensor_MPU9250::_register_read(uint8_t reg) { - uint8_t val; - _bus->read8(reg, &val); + uint8_t val = 0; + + reg |= _read_flag; + _dev->read_registers(reg, &val, 1); + return val; } -/* - write an 8 bit register - */ void AP_InertialSensor_MPU9250::_register_write(uint8_t reg, uint8_t val) { - _bus->write8(reg, val); + _dev->write_register(reg, val); } /* - initialise the sensor configuration registers + useful when debugging SPI bus errors */ +void AP_InertialSensor_MPU9250::_register_write_check(uint8_t reg, uint8_t val) +{ + uint8_t readed; + _register_write(reg, val); + readed = _register_read(reg); + if (readed != val){ + hal.console->printf("Values doesn't match; written: %02x; read: %02x ", val, readed); + } +#if MPU9250_DEBUG + hal.console->printf("Values written: %02x; readed: %02x ", val, readed); +#endif +} + bool AP_InertialSensor_MPU9250::_hardware_init(void) { - // we need to suspend timers to prevent other SPI drivers grabbing - // the bus while we do the long initialisation - hal.scheduler->suspend_timer_procs(); - - if (!_bus_sem->take(100)) { - hal.console->printf("MPU9250: Unable to get semaphore"); - return false; + if (!_dev->get_semaphore()->take(100)) { + AP_HAL::panic("MPU6000: Unable to get semaphore"); } // initially run the bus at low speed - _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + _dev->set_speed(AP_HAL::Device::SPEED_LOW); uint8_t value = _register_read(MPUREG_WHOAMI); if (value != MPUREG_WHOAMI_MPU9250 && value != MPUREG_WHOAMI_MPU9255) { @@ -560,7 +492,8 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) uint8_t user_ctrl = _register_read(MPUREG_USER_CTRL); /* First disable the master I2C to avoid hanging the slaves on the - * auxiliary I2C bus */ + * aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus + * is used */ if (user_ctrl & BIT_USER_CTRL_I2C_MST_EN) { _register_write(MPUREG_USER_CTRL, user_ctrl & ~BIT_USER_CTRL_I2C_MST_EN); hal.scheduler->delay(10); @@ -570,8 +503,12 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) _register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET); hal.scheduler->delay(100); - // bus-dependent initialization - _bus->init(); + /* bus-dependent initialization */ + if (_bus_type == BUS_TYPE_SPI) { + /* Disable I2C bus if SPI selected (Recommended in Datasheet to be + * done just after the device is reset) */ + _register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS); + } // Wake up device and select GyroZ clock. Note that the // MPU9250 starts up in sleep mode, and it can take some time @@ -585,71 +522,31 @@ bool AP_InertialSensor_MPU9250::_hardware_init(void) } hal.scheduler->delay(10); - uint8_t status = _register_read(MPUREG_INT_STATUS); - if ((status & BIT_RAW_RDY_INT) != 0) { + if (_data_ready()) { break; } + #if MPU9250_DEBUG _dump_registers(); #endif } - if (tries == 5) { hal.console->println("Failed to boot MPU9250 5 times"); goto fail_tries; } - _register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode - - // used no filter of 256Hz on the sensor, then filter using - // the 2-pole software filter - _register_write(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2); - - // set sample rate to 1kHz, and use the 2 pole filter to give the - // desired rate - _register_write(MPUREG_SMPLRT_DIV, DEFAULT_SMPLRT_DIV); - _register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s - - // RM-MPU-9250A-00.pdf, pg. 15, select accel full scale 16g - _register_write(MPUREG_ACCEL_CONFIG,3<<3); - - // configure interrupt to fire when new data arrives - _register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); - - // clear interrupt on any read, and hold the data ready pin high - // until we clear the interrupt - value = _register_read(MPUREG_INT_PIN_CFG); - value |= BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN; - _register_write(MPUREG_INT_PIN_CFG, value); - - // now that we have initialized, we set the SPI bus speed to high - _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - - _bus_sem->give(); - - hal.scheduler->resume_timer_procs(); + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + _dev->get_semaphore()->give(); return true; fail_tries: fail_whoami: - _bus_sem->give(); - hal.scheduler->resume_timer_procs(); - _bus->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _dev->get_semaphore()->give(); + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); return false; } -AuxiliaryBus *AP_InertialSensor_MPU9250::get_auxiliary_bus() -{ - if (_auxiliar_bus) - return _auxiliar_bus; - - if (_bus->has_auxiliary_bus()) - _auxiliar_bus = new AP_MPU9250_AuxiliaryBus(*this); - - return _auxiliar_bus; -} - #if MPU9250_DEBUG // dump all config registers - used for debug void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus) @@ -666,60 +563,6 @@ void AP_InertialSensor_MPU9250::_dump_registers(AP_MPU9250_BusDriver *bus) } #endif -AP_MPU9250_AuxiliaryBus::AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend) - : AuxiliaryBus(backend, 4) -{ -} - -AP_HAL::Semaphore *AP_MPU9250_AuxiliaryBus::get_semaphore() -{ - return AP_InertialSensor_MPU9250::from(_ins_backend)._bus_sem; -} - -AuxiliaryBusSlave *AP_MPU9250_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance) -{ - /* Enable slaves on MPU9250 if this is the first time */ - if (_ext_sens_data == 0) - _configure_slaves(); - - return new AP_MPU9250_AuxiliaryBusSlave(*this, addr, instance); -} - -void AP_MPU9250_AuxiliaryBus::_configure_slaves() -{ - AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_ins_backend); - - /* Enable the I2C master to slaves on the auxiliary I2C bus*/ - uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL); - backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN); - - /* stop condition between reads; clock at 400kHz */ - backend._register_write(MPUREG_I2C_MST_CTRL, I2C_MST_CLOCK_400KHZ | I2C_MST_P_NSR); - - /* Hard-code divider for internal sample rate, 1 kHz, resulting in a - * sample rate of 100Hz */ - backend._register_write(MPUREG_I2C_SLV4_CTRL, 9); - - /* All slaves are subject to the sample rate */ - backend._register_write(MPUREG_I2C_MST_DELAY_CTRL, I2C_SLV0_DLY_EN - | I2C_SLV1_DLY_EN | I2C_SLV2_DLY_EN | I2C_SLV3_DLY_EN); -} - -int AP_MPU9250_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave, - uint8_t reg, uint8_t size) -{ - if (_ext_sens_data + size > MAX_EXT_SENS_DATA) - return -1; - - AP_MPU9250_AuxiliaryBusSlave *mpu_slave = - static_cast(slave); - mpu_slave->_set_passthrough(reg, size); - mpu_slave->_ext_sens_data = _ext_sens_data; - _ext_sens_data += size; - - return 0; -} - AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance) : AuxiliaryBusSlave(bus, addr, instance) @@ -733,7 +576,7 @@ AP_MPU9250_AuxiliaryBusSlave::AP_MPU9250_AuxiliaryBusSlave(AuxiliaryBus &bus, ui int AP_MPU9250_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) { - AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); + auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); uint8_t addr; /* Ensure the slave read/write is disabled before changing the registers */ @@ -764,14 +607,15 @@ int AP_MPU9250_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf, } int r = _set_passthrough(reg, size); - if (r < 0) + if (r < 0) { return r; + } /* wait the value to be read from the slave and read it back */ hal.scheduler->delay(10); - AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); - backend._bus->read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size); + auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); + backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, size); /* disable new reads */ backend._register_write(_mpu9250_ctrl, 0); @@ -787,13 +631,14 @@ int AP_MPU9250_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val) } int r = _set_passthrough(reg, 1, &val); - if (r < 0) + if (r < 0) { return r; + } /* wait the value to be written to the slave */ hal.scheduler->delay(10); - AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); + auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); /* disable new writes */ backend._register_write(_mpu9250_ctrl, 0); @@ -808,10 +653,68 @@ int AP_MPU9250_AuxiliaryBusSlave::read(uint8_t *buf) return -1; } - AP_InertialSensor_MPU9250 &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); - backend._bus->read_block(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size); + auto &backend = AP_InertialSensor_MPU9250::from(_bus.get_backend()); + backend._block_read(MPUREG_EXT_SENS_DATA_00 + _ext_sens_data, buf, _sample_size); return 0; } -#endif // CONFIG_HAL_BOARD +/* MPU9250 provides up to 5 slave devices, but the 5th is way too different to + * configure and is seldom used */ +AP_MPU9250_AuxiliaryBus::AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend) + : AuxiliaryBus(backend, 4) +{ +} + +AP_HAL::Semaphore *AP_MPU9250_AuxiliaryBus::get_semaphore() +{ + return AP_InertialSensor_MPU9250::from(_ins_backend)._dev->get_semaphore(); +} + +AuxiliaryBusSlave *AP_MPU9250_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance) +{ + /* Enable slaves on MPU9250 if this is the first time */ + if (_ext_sens_data == 0) + _configure_slaves(); + + return new AP_MPU9250_AuxiliaryBusSlave(*this, addr, instance); +} + +void AP_MPU9250_AuxiliaryBus::_configure_slaves() +{ + auto &backend = AP_InertialSensor_MPU9250::from(_ins_backend); + + /* Enable the I2C master to slaves on the auxiliary I2C bus*/ + uint8_t user_ctrl = backend._register_read(MPUREG_USER_CTRL); + backend._register_write(MPUREG_USER_CTRL, user_ctrl | BIT_USER_CTRL_I2C_MST_EN); + + /* stop condition between reads; clock at 400kHz */ + backend._register_write(MPUREG_I2C_MST_CTRL, + I2C_MST_CLOCK_400KHZ | I2C_MST_P_NSR); + + /* Hard-code divider for internal sample rate, 1 kHz, resulting in a + * sample rate of 100Hz */ + backend._register_write(MPUREG_I2C_SLV4_CTRL, 9); + + /* All slaves are subject to the sample rate */ + backend._register_write(MPUREG_I2C_MST_DELAY_CTRL, + I2C_SLV0_DLY_EN | I2C_SLV1_DLY_EN | + I2C_SLV2_DLY_EN | I2C_SLV3_DLY_EN); +} + +int AP_MPU9250_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave, + uint8_t reg, uint8_t size) +{ + if (_ext_sens_data + size > MAX_EXT_SENS_DATA) { + return -1; + } + + AP_MPU9250_AuxiliaryBusSlave *mpu_slave = + static_cast(slave); + mpu_slave->_set_passthrough(reg, size); + mpu_slave->_ext_sens_data = _ext_sens_data; + _ext_sens_data += size; + + return 0; +} +#endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h index 3e6a3868bf..f341e3ae83 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_MPU9250.h @@ -4,6 +4,8 @@ #include #include +#include +#include #include #include #include @@ -18,129 +20,87 @@ class AP_MPU9250_AuxiliaryBusSlave; // enable debug to see a register dump on startup #define MPU9250_DEBUG 0 -class AP_MPU9250_BusDriver -{ -public: - virtual ~AP_MPU9250_BusDriver() { }; - virtual void init() = 0; - virtual void read8(uint8_t reg, uint8_t *val) = 0; - virtual void write8(uint8_t reg, uint8_t val) = 0; - virtual void read_block(uint8_t reg, uint8_t *val, uint8_t count) = 0; - virtual void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) = 0; - virtual bool read_data_transaction(uint8_t* samples, - uint8_t &n_samples) = 0; - virtual AP_HAL::Semaphore* get_semaphore() = 0; - virtual bool has_auxiliary_bus() = 0; -}; - class AP_InertialSensor_MPU9250 : public AP_InertialSensor_Backend { friend AP_MPU9250_AuxiliaryBus; friend AP_MPU9250_AuxiliaryBusSlave; public: - - AP_InertialSensor_MPU9250(AP_InertialSensor &imu, AP_MPU9250_BusDriver *bus); - - /* update accel and gyro state */ - bool update(); - - AuxiliaryBus *get_auxiliary_bus(); + virtual ~AP_InertialSensor_MPU9250(); static AP_InertialSensor_MPU9250 &from(AP_InertialSensor_Backend &backend) { return static_cast(backend); } - // detect the sensor - static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu, AP_HAL::SPIDeviceDriver *spi); - static AP_InertialSensor_Backend *detect_i2c(AP_InertialSensor &imu, - AP_HAL::I2CDriver *i2c, - uint8_t addr); + static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev); + + static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev); + + /* update accel and gyro state */ + bool update(); + + /* + * Return an AuxiliaryBus if the bus driver allows it + */ + AuxiliaryBus *get_auxiliary_bus() override; + + void start() override; private: - static AP_InertialSensor_Backend *_detect(AP_InertialSensor &_imu, - AP_MPU9250_BusDriver *bus, - int16_t id); + enum bus_type { + BUS_TYPE_I2C = 0, + BUS_TYPE_SPI, + }; - bool _init_sensor(); - void _read_data_transaction(); - bool _data_ready(); - void _poll_data(void); - uint8_t _register_read( uint8_t reg ); - void _register_write( uint8_t reg, uint8_t val ); - bool _hardware_init(void); + AP_InertialSensor_MPU9250(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev, + enum bus_type bus_type, + uint8_t read_flag); - AP_MPU9250_BusDriver *_bus; - AP_HAL::Semaphore *_bus_sem; - AP_MPU9250_AuxiliaryBus *_auxiliar_bus = nullptr; +#if MPU9250_DEBUG + static void _dump_registers(); +#endif - // gyro and accel instances + bool _init(); + bool _hardware_init(); + + void _set_filter_register(uint16_t filter_hz); + bool _has_auxiliary_bus(); + + /* Read a single sample */ + void _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); + uint8_t _register_read(uint8_t reg); + void _register_write(uint8_t reg, uint8_t val ); + void _register_write_check(uint8_t reg, uint8_t val); + + void _accumulate(uint8_t *sample); + + // instance numbers of accel and gyro data uint8_t _gyro_instance; uint8_t _accel_instance; + const uint8_t _read_flag; + const enum bus_type _bus_type; + // The default rotation for the IMU, its value depends on how the IMU is // placed by default on the system enum Rotation _default_rotation; -#if MPU9250_DEBUG - static void _dump_registers(); -#endif -}; - -class AP_MPU9250_BusDriver_SPI : public AP_MPU9250_BusDriver -{ -public: - AP_MPU9250_BusDriver_SPI(AP_HAL::SPIDeviceDriver *spi); - void init(); - void read8(uint8_t reg, uint8_t *val); - void write8(uint8_t reg, uint8_t val); - void read_block(uint8_t reg, uint8_t *val, uint8_t count); - void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed); - bool read_data_transaction(uint8_t* samples, uint8_t &n_samples); - AP_HAL::Semaphore* get_semaphore(); - bool has_auxiliary_bus(); - -private: - AP_HAL::SPIDeviceDriver *_spi; -}; - -class AP_MPU9250_BusDriver_I2C : public AP_MPU9250_BusDriver -{ -public: - AP_MPU9250_BusDriver_I2C(AP_HAL::I2CDriver *i2c, uint8_t addr); - void init(); - void read8(uint8_t reg, uint8_t *val); - void write8(uint8_t reg, uint8_t val); - void read_block(uint8_t reg, uint8_t *val, uint8_t count); - void set_bus_speed(AP_HAL::SPIDeviceDriver::bus_speed speed) {}; - bool read_data_transaction(uint8_t* samples, uint8_t &n_samples); - AP_HAL::Semaphore* get_semaphore(); - bool has_auxiliary_bus(); - -private: - uint8_t _addr; - AP_HAL::I2CDriver *_i2c; -}; - -class AP_MPU9250_AuxiliaryBus : public AuxiliaryBus -{ - friend class AP_InertialSensor_MPU9250; - -public: - AP_HAL::Semaphore *get_semaphore() override; - -protected: - AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend); - - AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance); - int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg, - uint8_t size); - -private: - void _configure_slaves(); - - static const uint8_t MAX_EXT_SENS_DATA = 24; - uint8_t _ext_sens_data = 0; + AP_HAL::OwnPtr _dev; + AP_MPU9250_AuxiliaryBus *_auxiliary_bus; }; class AP_MPU9250_AuxiliaryBusSlave : public AuxiliaryBusSlave @@ -165,3 +125,24 @@ private: uint8_t _ext_sens_data = 0; }; + +class AP_MPU9250_AuxiliaryBus : public AuxiliaryBus +{ + friend class AP_InertialSensor_MPU9250; + +public: + AP_HAL::Semaphore *get_semaphore() override; + +protected: + AP_MPU9250_AuxiliaryBus(AP_InertialSensor_MPU9250 &backend); + + AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance); + int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg, + uint8_t size); + +private: + void _configure_slaves(); + + static const uint8_t MAX_EXT_SENS_DATA = 24; + uint8_t _ext_sens_data = 0; +};