mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Lucas De Marchi
parent
5164fbb81b
commit
cf2d866d51
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include <utility>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
|
@ -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<AP_HAL::Device> 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<AP_HAL::Device> 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
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/Device.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#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<AP_HAL::Device> 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<AP_HAL::Device> 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<AP_HAL::Device> _dev;
|
||||
|
||||
float _mag_range_scale;
|
||||
float _mag_x_accum;
|
||||
|
|
|
|||
Loading…
Reference in New Issue