/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #include #include #include #include "AP_Compass_LSM303D.h" extern const AP_HAL::HAL &hal; #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT #define LSM303D_DRDY_M_PIN RPI_GPIO_27 #endif #endif #ifndef LSM303D_DRDY_M_PIN #define LSM303D_DRDY_M_PIN -1 #endif /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) #define ADDR_INCREMENT (1<<6) /* register addresses: A: accel, M: mag, T: temp */ #define ADDR_WHO_AM_I 0x0F #define WHO_I_AM 0x49 #define ADDR_OUT_TEMP_L 0x05 #define ADDR_OUT_TEMP_H 0x06 #define ADDR_STATUS_M 0x07 #define ADDR_OUT_X_L_M 0x08 #define ADDR_OUT_X_H_M 0x09 #define ADDR_OUT_Y_L_M 0x0A #define ADDR_OUT_Y_H_M 0x0B #define ADDR_OUT_Z_L_M 0x0C #define ADDR_OUT_Z_H_M 0x0D #define ADDR_INT_CTRL_M 0x12 #define ADDR_INT_SRC_M 0x13 #define ADDR_REFERENCE_X 0x1c #define ADDR_REFERENCE_Y 0x1d #define ADDR_REFERENCE_Z 0x1e #define ADDR_STATUS_A 0x27 #define ADDR_OUT_X_L_A 0x28 #define ADDR_OUT_X_H_A 0x29 #define ADDR_OUT_Y_L_A 0x2A #define ADDR_OUT_Y_H_A 0x2B #define ADDR_OUT_Z_L_A 0x2C #define ADDR_OUT_Z_H_A 0x2D #define ADDR_CTRL_REG0 0x1F #define ADDR_CTRL_REG1 0x20 #define ADDR_CTRL_REG2 0x21 #define ADDR_CTRL_REG3 0x22 #define ADDR_CTRL_REG4 0x23 #define ADDR_CTRL_REG5 0x24 #define ADDR_CTRL_REG6 0x25 #define ADDR_CTRL_REG7 0x26 #define ADDR_FIFO_CTRL 0x2e #define ADDR_FIFO_SRC 0x2f #define ADDR_IG_CFG1 0x30 #define ADDR_IG_SRC1 0x31 #define ADDR_IG_THS1 0x32 #define ADDR_IG_DUR1 0x33 #define ADDR_IG_CFG2 0x34 #define ADDR_IG_SRC2 0x35 #define ADDR_IG_THS2 0x36 #define ADDR_IG_DUR2 0x37 #define ADDR_CLICK_CFG 0x38 #define ADDR_CLICK_SRC 0x39 #define ADDR_CLICK_THS 0x3a #define ADDR_TIME_LIMIT 0x3b #define ADDR_TIME_LATENCY 0x3c #define ADDR_TIME_WINDOW 0x3d #define ADDR_ACT_THS 0x3e #define ADDR_ACT_DUR 0x3f #define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4)) #define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4)) #define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4)) #define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4)) #define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4)) #define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4)) #define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4)) #define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4)) #define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4)) #define REG1_BDU_UPDATE (1<<3) #define REG1_Z_ENABLE_A (1<<2) #define REG1_Y_ENABLE_A (1<<1) #define REG1_X_ENABLE_A (1<<0) #define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6)) #define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6)) #define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6)) #define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6)) #define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6)) #define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3)) #define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3)) #define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3)) #define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3)) #define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3)) #define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3)) #define REG5_ENABLE_T (1<<7) #define REG5_RES_HIGH_M ((1<<6) | (1<<5)) #define REG5_RES_LOW_M ((0<<6) | (0<<5)) #define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2)) #define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2)) #define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2)) #define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2)) #define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2)) #define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2)) #define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2)) #define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2)) #define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5)) #define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5)) #define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5)) #define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5)) #define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5)) #define REG7_CONT_MODE_M ((0<<1) | (0<<0)) #define INT_CTRL_M 0x12 #define INT_SRC_M 0x13 #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 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_HAL::OwnPtr dev, enum Rotation rotation) { if (!dev) { return nullptr; } AP_Compass_LSM303D *sensor = new AP_Compass_LSM303D(compass, std::move(dev)); if (!sensor || !sensor->init(rotation)) { delete sensor; return nullptr; } return sensor; } uint8_t AP_Compass_LSM303D::_register_read(uint8_t reg) { uint8_t val = 0; reg |= DIR_READ; _dev->read_registers(reg, &val, 1); return val; } 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) { _dev->write_register(reg, val); } void AP_Compass_LSM303D::_register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits) { uint8_t val; val = _register_read(reg); val &= ~clearbits; val |= setbits; _register_write(reg, val); } /** * Return true if the LSM303D has new data available for both the mag and * the accels. */ bool AP_Compass_LSM303D::_data_ready() { return _drdy_pin_m == nullptr || (_drdy_pin_m->read() != 0); } // Read Sensor data 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->printf("LSM303D _read_data_transaction_accel: _reg7_expected unexpected\n"); return false; } if (!_data_ready()) { return false; } if (!_block_read(ADDR_STATUS_M, (uint8_t *) &rx, sizeof(rx))) { return false; } /* check for overrun */ if ((rx.status & 0x70) != 0) { return false; } if (rx.x == 0 && rx.y == 0 && rx.z == 0) { return false; } _mag_x = rx.x; _mag_y = rx.y; _mag_z = rx.z; return true; } bool AP_Compass_LSM303D::init(enum Rotation rotation) { if (LSM303D_DRDY_M_PIN >= 0) { _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN); _drdy_pin_m->mode(HAL_GPIO_INPUT); } bool success = _hardware_init(); if (!success) { return false; } _initialised = true; /* register the compass instance in the frontend */ _compass_instance = register_compass(); set_rotation(_compass_instance, rotation); _dev->set_device_type(DEVTYPE_LSM303D); set_dev_id(_compass_instance, _dev->get_bus_id()); #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 // read at 100Hz _dev->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_LSM303D::_update, void)); return true; } bool AP_Compass_LSM303D::_hardware_init() { if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { AP_HAL::panic("LSM303D: Unable to get semaphore"); } // initially run the bus at low speed _dev->set_speed(AP_HAL::Device::SPEED_LOW); // 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; } uint8_t tries; for (tries = 0; tries < 5; tries++) { // ensure the chip doesn't interpret any other bus traffic as I2C _disable_i2c(); /* enable mag */ _reg7_expected = REG7_CONT_MODE_M; _register_write(ADDR_CTRL_REG7, _reg7_expected); _register_write(ADDR_CTRL_REG5, REG5_RES_HIGH_M); // 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->printf("Failed to boot LSM303D 5 times\n"); 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() { if (!_read_sample()) { return; } Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * _mag_range_scale; // rotate raw_field from sensor frame to body frame rotate_field(raw_field, _compass_instance); // publish raw_field (uncorrected point sample) for calibration use publish_raw_field(raw_field, _compass_instance); // correct raw_field for known errors correct_field(raw_field, _compass_instance); if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { _mag_x_accum += raw_field.x; _mag_y_accum += raw_field.y; _mag_z_accum += raw_field.z; _accum_count++; if (_accum_count == 10) { _mag_x_accum /= 2; _mag_y_accum /= 2; _mag_z_accum /= 2; _accum_count = 5; } _sem->give(); } } // Read Sensor data void AP_Compass_LSM303D::read() { if (!_initialised) { return; } if (!_sem->take_nonblocking()) { return; } if (_accum_count == 0) { /* We're not ready to publish*/ _sem->give(); return; } Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum); field /= _accum_count; _accum_count = 0; _mag_x_accum = _mag_y_accum = _mag_z_accum = 0; _sem->give(); publish_filtered_field(field, _compass_instance); } void AP_Compass_LSM303D::_disable_i2c() { // TODO: use the register names uint8_t a = _register_read(0x02); _register_write(0x02, (0x10 | a)); a = _register_read(0x02); _register_write(0x02, (0xF7 & a)); a = _register_read(0x15); _register_write(0x15, (0x80 | a)); a = _register_read(0x02); _register_write(0x02, (0xE7 & a)); } bool AP_Compass_LSM303D::_mag_set_range(uint8_t max_ga) { uint8_t setbits = 0; uint8_t clearbits = REG6_FULL_SCALE_BITS_M; float new_scale_ga_digit = 0.0f; if (max_ga == 0) { max_ga = 12; } if (max_ga <= 2) { _mag_range_ga = 2; setbits |= REG6_FULL_SCALE_2GA_M; new_scale_ga_digit = 0.080f; } else if (max_ga <= 4) { _mag_range_ga = 4; setbits |= REG6_FULL_SCALE_4GA_M; new_scale_ga_digit = 0.160f; } else if (max_ga <= 8) { _mag_range_ga = 8; setbits |= REG6_FULL_SCALE_8GA_M; new_scale_ga_digit = 0.320f; } else if (max_ga <= 12) { _mag_range_ga = 12; setbits |= REG6_FULL_SCALE_12GA_M; new_scale_ga_digit = 0.479f; } else { return false; } _mag_range_scale = new_scale_ga_digit; _register_modify(ADDR_CTRL_REG6, clearbits, setbits); return true; } bool AP_Compass_LSM303D::_mag_set_samplerate(uint16_t frequency) { uint8_t setbits = 0; uint8_t clearbits = REG5_RATE_BITS_M; if (frequency == 0) { frequency = 100; } if (frequency <= 25) { setbits |= REG5_RATE_25HZ_M; _mag_samplerate = 25; } else if (frequency <= 50) { setbits |= REG5_RATE_50HZ_M; _mag_samplerate = 50; } else if (frequency <= 100) { setbits |= REG5_RATE_100HZ_M; _mag_samplerate = 100; } else { return false; } _register_modify(ADDR_CTRL_REG5, clearbits, setbits); return true; }