From cf2d866d511b618c94a5e580e52d7cc272359233 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 21 Mar 2016 12:01:53 -0300 Subject: [PATCH] AP_Compass: LSM303D: used AP_HAL::Device interface Initialization was also changed a little bit so we don't try to initialize 25 times. We rather use the same methods as in the AP_InertialSensor drivers. Also move up the call to is_zero() in read_raw so we don't set _mag_[x|y|z] in case of failure. --- libraries/AP_Compass/AP_Compass.cpp | 2 +- libraries/AP_Compass/AP_Compass_LSM303D.cpp | 177 +++++++++----------- libraries/AP_Compass/AP_Compass_LSM303D.h | 18 +- 3 files changed, 96 insertions(+), 101 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index a4c1094728..f89680337d 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -441,7 +441,7 @@ void Compass::_detect_backends(void) _add_backend(AP_Compass_QURT::detect(*this)); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_RASPILOT _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(HAL_COMPASS_HMC5843_I2C_BUS, HAL_COMPASS_HMC5843_I2C_ADDR))); - _add_backend(AP_Compass_LSM303D::probe(*this)); + _add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device("lsm9ds0_am"))); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_BH // detect_mpu9250() failed will cause panic if no actual mpu9250 backend, // in BH, only one compass should be detected diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp index c56a87442b..375edc57f7 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp @@ -13,6 +13,7 @@ You should have received a copy of the GNU General Public License along with this program. If not, see . */ +#include #include #include @@ -153,14 +154,16 @@ extern const AP_HAL::HAL &hal; #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 -AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass) +AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass, AP_HAL::OwnPtr dev) : AP_Compass_Backend(compass) + , _dev(std::move(dev)) { } -AP_Compass_Backend *AP_Compass_LSM303D::probe(Compass &compass) +AP_Compass_Backend *AP_Compass_LSM303D::probe(Compass &compass, + AP_HAL::OwnPtr dev) { - AP_Compass_LSM303D *sensor = new AP_Compass_LSM303D(compass); + AP_Compass_LSM303D *sensor = new AP_Compass_LSM303D(compass, std::move(dev)); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -171,26 +174,23 @@ AP_Compass_Backend *AP_Compass_LSM303D::probe(Compass &compass) uint8_t AP_Compass_LSM303D::_register_read(uint8_t reg) { - uint8_t addr = reg | 0x80; // Set most significant bit + uint8_t val = 0; - uint8_t tx[2]; - uint8_t rx[2]; + reg |= DIR_READ; + _dev->read_registers(reg, &val, 1); - tx[0] = addr; - tx[1] = 0; - _spi->transaction(tx, rx, 2); + return val; +} - return rx[1]; +bool AP_Compass_LSM303D::_block_read(uint8_t reg, uint8_t *buf, uint32_t size) +{ + reg |= DIR_READ | ADDR_INCREMENT; + return _dev->read_registers(reg, buf, size); } void AP_Compass_LSM303D::_register_write(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); + _dev->write_register(reg, val); } void AP_Compass_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits) @@ -214,8 +214,15 @@ bool AP_Compass_LSM303D::_data_ready() // Read Sensor data -bool AP_Compass_LSM303D::_read_raw() +bool AP_Compass_LSM303D::_read_sample() { + struct PACKED { + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } rx; + if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) { hal.console->println("LSM303D _read_data_transaction_accel: _reg7_expected unexpected"); return false; @@ -225,36 +232,18 @@ bool AP_Compass_LSM303D::_read_raw() return false; } - struct PACKED { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_mag_report_tx; - - struct PACKED { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_mag_report_rx; - - /* fetch data from the sensor */ - memset(&raw_mag_report_tx, 0, sizeof(raw_mag_report_tx)); - memset(&raw_mag_report_rx, 0, sizeof(raw_mag_report_rx)); - raw_mag_report_tx.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; - _spi->transaction((uint8_t *)&raw_mag_report_tx, (uint8_t *)&raw_mag_report_rx, sizeof(raw_mag_report_tx)); - - _mag_x = raw_mag_report_rx.x; - _mag_y = raw_mag_report_rx.y; - _mag_z = raw_mag_report_rx.z; - - if (is_zero(_mag_x) && is_zero(_mag_y) && is_zero(_mag_z)) { + if (!_block_read(ADDR_STATUS_M, (uint8_t *) &rx, sizeof(rx))) { return false; } + if (is_zero(rx.x) && is_zero(rx.y) && is_zero(rx.z)) { + return false; + } + + _mag_x = rx.x; + _mag_y = rx.y; + _mag_z = rx.z; + return true; } @@ -265,40 +254,16 @@ bool AP_Compass_LSM303D::init() return false; } - hal.scheduler->suspend_timer_procs(); - - _spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_AM); - _spi_sem = _spi->get_semaphore(); - _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN); _drdy_pin_m->mode(HAL_GPIO_INPUT); - // Test WHOAMI - uint8_t whoami = _register_read(ADDR_WHO_AM_I); - if (whoami != WHO_I_AM) { - hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami); - AP_HAL::panic("LSM303D: bad WHOAMI"); - } + hal.scheduler->suspend_timer_procs(); + bool success = _hardware_init(); + hal.scheduler->resume_timer_procs(); - uint8_t tries = 0; - do { - // TODO: don't try to init 25 times - bool success = _hardware_init(); - if (success) { - hal.scheduler->delay(5+2); - if (!_spi_sem->take(100)) { - AP_HAL::panic("LSM303D: Unable to get semaphore"); - } - if (_data_ready()) { - _spi_sem->give(); - break; - } - _spi_sem->give(); - } - if (tries++ > 5) { - AP_HAL::panic("PANIC: failed to boot LSM303D 5 times"); - } - } while (1); + if (!success) { + return false; + } _initialised = true; @@ -307,43 +272,67 @@ bool AP_Compass_LSM303D::init() set_dev_id(_compass_instance, AP_COMPASS_TYPE_LSM303D); #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT + // FIXME: wrong way to force internal compass set_external(_compass_instance, false); #endif hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void)); - _spi_sem->give(); - hal.scheduler->resume_timer_procs(); - return true; } bool AP_Compass_LSM303D::_hardware_init() { - if (!_spi_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { AP_HAL::panic("LSM303D: Unable to get semaphore"); } // initially run the bus at low speed - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + _dev->set_speed(AP_HAL::Device::SPEED_LOW); - // ensure the chip doesn't interpret any other bus traffic as I2C - _disable_i2c(); + // Test WHOAMI + uint8_t whoami = _register_read(ADDR_WHO_AM_I); + if (whoami != WHO_I_AM) { + hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami); + goto fail_whoami; + } - /* enable mag */ - _reg7_expected = REG7_CONT_MODE_M; - _register_write(ADDR_CTRL_REG7, _reg7_expected); - _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - _register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 + uint8_t tries; + for (tries = 0; tries < 5; tries++) { + // ensure the chip doesn't interpret any other bus traffic as I2C + _disable_i2c(); - _mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); - _mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + /* enable mag */ + _reg7_expected = REG7_CONT_MODE_M; + _register_write(ADDR_CTRL_REG7, _reg7_expected); + _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - // now that we have initialised, we set the SPI bus speed to high - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - _spi_sem->give(); + // DRDY on MAG on INT2 + _register_write(ADDR_CTRL_REG4, 0x04); + + _mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + _mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + + hal.scheduler->delay(10); + if (_data_ready()) { + break; + } + } + if (tries == 5) { + hal.console->println("Failed to boot LSM303D 5 times"); + goto fail_tries; + } + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + _dev->get_semaphore()->give(); return true; + +fail_tries: +fail_whoami: + _dev->get_semaphore()->give(); + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + return false; } void AP_Compass_LSM303D::_update() @@ -355,11 +344,11 @@ void AP_Compass_LSM303D::_update() return; } - if (!_spi_sem->take_nonblocking()) { + if (!_dev->get_semaphore()->take_nonblocking()) { return; } - if (!_read_raw()) { + if (!_read_sample()) { goto fail; } @@ -388,7 +377,7 @@ void AP_Compass_LSM303D::_update() _last_update_timestamp = AP_HAL::micros(); fail: - _spi_sem->give(); + _dev->get_semaphore()->give(); } // Read Sensor data diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.h b/libraries/AP_Compass/AP_Compass_LSM303D.h index 71c68d43b4..457f57f4ab 100644 --- a/libraries/AP_Compass/AP_Compass_LSM303D.h +++ b/libraries/AP_Compass/AP_Compass_LSM303D.h @@ -3,6 +3,7 @@ #include #include +#include #include #include "AP_Compass.h" @@ -11,18 +12,24 @@ class AP_Compass_LSM303D : public AP_Compass_Backend { public: - static AP_Compass_Backend *probe(Compass &compass); + static AP_Compass_Backend *probe(Compass &compass, + AP_HAL::OwnPtr dev); bool init() override; void read() override; -private: - AP_Compass_LSM303D(Compass &compass); + virtual ~AP_Compass_LSM303D() { } + +private: + AP_Compass_LSM303D(Compass &compass, AP_HAL::OwnPtr dev); - bool _read_raw(); uint8_t _register_read(uint8_t reg); void _register_write(uint8_t reg, uint8_t val); void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits); + bool _block_read(uint8_t reg, uint8_t *buf, uint32_t size); + + bool _read_sample(); + bool _data_ready(); bool _hardware_init(); void _update(); @@ -30,9 +37,8 @@ private: bool _mag_set_range(uint8_t max_ga); bool _mag_set_samplerate(uint16_t frequency); - AP_HAL::SPIDeviceDriver *_spi; - AP_HAL::Semaphore *_spi_sem; AP_HAL::DigitalSource *_drdy_pin_m; + AP_HAL::OwnPtr _dev; float _mag_range_scale; float _mag_x_accum;