diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp new file mode 100644 index 0000000000..0826e754ae --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp @@ -0,0 +1,826 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/**************************************************************************** + * + * Coded by Víctor Mayoral Vilches using + * lsm3030d.cpp from the PX4 Development Team. + * + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "AP_InertialSensor_LSM303D.h" + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_APM2 + #define LSM303D_DRDY_PIN 70 +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF + #include "../AP_HAL_Linux/GPIO.h" + #define LSM303D_DRDY_X_PIN BBB_P8_8 // ACCEL DRDY + #define LSM303D_DRDY_M_PIN BBB_P8_10 // MAGNETOMETER DRDY + #endif +#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 + +/* default values for this device */ +#define LSM303D_ACCEL_DEFAULT_RANGE_G 8 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 + +#define LSM303D_ONE_G 9.80665f + + +AP_InertialSensor_LSM303D::AP_InertialSensor_LSM303D() : + AP_InertialSensor(), + _drdy_pin_x(NULL), + _drdy_pin_m(NULL), + _initialised(false), + _LSM303D_product_id(AP_PRODUCT_ID_NONE) +{ +} + +uint16_t AP_InertialSensor_LSM303D::_init_sensor( Sample_rate sample_rate ) +{ + if (_initialised) return _LSM303D_product_id; + _initialised = true; + + _spi = hal.spi->device(AP_HAL::SPIDevice_LSM303D); + _spi_sem = _spi->get_semaphore(); + +// This device has mag and accel +#ifdef LSM303D_DRDY_X_PIN + _drdy_pin_x = hal.gpio->channel(LSM303D_DRDY_X_PIN); + _drdy_pin_x->mode(HAL_GPIO_INPUT); +#endif + +#ifdef LSM303D_DRDY_M_PIN + _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN); + _drdy_pin_m->mode(HAL_GPIO_INPUT); +#endif + + hal.scheduler->suspend_timer_procs(); + + // Test WHOAMI + uint8_t whoami = _register_read(ADDR_WHO_AM_I); + if (whoami != WHO_I_AM) { + // TODO: we should probably accept multiple chip + // revisions. This is the one on the PXF + hal.console->printf("LSM303D: unexpected WHOAMI 0x%x\n", (unsigned)whoami); + hal.scheduler->panic("LSM303D: bad WHOAMI"); + } + + uint8_t tries = 0; + do { + bool success = _hardware_init(sample_rate); + if (success) { + hal.scheduler->delay(5+2); + if (!_spi_sem->take(100)) { + hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore")); + } + if (_data_ready()) { + _spi_sem->give(); + break; + } else { + hal.console->println_P( + PSTR("LSM303D startup failed: no data ready")); + } + _spi_sem->give(); + } + if (tries++ > 5) { + hal.scheduler->panic(PSTR("PANIC: failed to boot LSM303D 5 times")); + } + } while (1); + + hal.scheduler->resume_timer_procs(); + + + /* read the first lot of data. + * _read_data_transaction requires the spi semaphore to be taken by + * its caller. */ + _last_sample_time_micros = hal.scheduler->micros(); + hal.scheduler->delay(10); + if (_spi_sem->take(100)) { + _read_data_transaction(); + _spi_sem->give(); + } + + // start the timer process to read samples + hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_InertialSensor_LSM303D::_poll_data)); + +#if LSM303D_DEBUG + _dump_registers(); +#endif + return _LSM303D_product_id; +} + +/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ + +bool AP_InertialSensor_LSM303D::wait_for_sample(uint16_t timeout_ms) +{ + if (_sample_available()) { + return true; + } + uint32_t start = hal.scheduler->millis(); + while ((hal.scheduler->millis() - start) < timeout_ms) { + hal.scheduler->delay_microseconds(100); + if (_sample_available()) { + return true; + } + } + return false; +} + +bool AP_InertialSensor_LSM303D::update( void ) +{ + // wait for at least 1 sample + if (!wait_for_sample(1000)) { + return false; + } + + // disable timer procs for mininum time + hal.scheduler->suspend_timer_procs(); + + _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); + // _mag[0] = Vector3f(_mag_sum.x, _mag_sum.y, _mag_sum.z); + + _num_samples = _sum_count; + _accel_sum.zero(); + _mag_sum.zero(); + _sum_count = 0; + hal.scheduler->resume_timer_procs(); + + _accel[0].rotate(_board_orientation); + // TODO change this for the corresponding value + // _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; + + // Vector3f accel_scale = _accel_scale[0].get(); + // _accel[0].x *= accel_scale.x; + // _accel[0].y *= accel_scale.y; + // _accel[0].z *= accel_scale.z; + // _accel[0] -= _accel_offset[0]; + + // TODO similarly put mag values in _mag and scale them + + // if (_last_filter_hz != _LSM303D_filter) { + // if (_spi_sem->take(10)) { + // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + // _set_filter_register(_LSM303D_filter, 0); + // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + // _error_count = 0; + // _spi_sem->give(); + // } + // } + + return true; +} + +/*================ HARDWARE FUNCTIONS ==================== */ + +/** + * Return true if the LSM303D has new data available for both the mag and the accels. + * + * We use the data ready pin if it is available. Otherwise, read the + * status register. + */ +bool AP_InertialSensor_LSM303D::_data_ready() +{ + if (_drdy_pin_m && _drdy_pin_x) { + return (_drdy_pin_m->read() && _drdy_pin_x->read()) != 0; + } +} + +/** + * Timer process to poll for new data from the LSM303D. + */ +void AP_InertialSensor_LSM303D::_poll_data(void) +{ + if (hal.scheduler->in_timerprocess()) { + 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 (_data_ready()) { + _last_sample_time_micros = hal.scheduler->micros(); + _read_data_transaction(); + } + _spi_sem->give(); + } else { + /* Synchronous read - take semaphore */ + if (_spi_sem->take(10)) { + if (_data_ready()) { + _last_sample_time_micros = hal.scheduler->micros(); + _read_data_transaction(); + } + _spi_sem->give(); + } else { + hal.scheduler->panic( + PSTR("PANIC: AP_InertialSensor_LSM303D::_poll_data " + "failed to take SPI semaphore synchronously")); + } + } +} + +void AP_InertialSensor_LSM303D::_read_data_transaction_accel() +{ + + if (_register_read(ADDR_CTRL_REG1) != _reg1_expected) { + hal.console->println_P( + PSTR("LSM303D _read_data_transaction_accel: _reg1_expected unexpected")); + // reset(); + return; + } + + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_accel_report; + + /* fetch data from the sensor */ + memset(&raw_accel_report, 0, sizeof(raw_accel_report)); + raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + _spi->transaction((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + + _accel_sum.x += raw_accel_report.x; + _accel_sum.y += raw_accel_report.y; + _accel_sum.z += raw_accel_report.z; +} + +void AP_InertialSensor_LSM303D::_read_data_transaction_mag() { + if (_register_read(ADDR_CTRL_REG7) != _reg7_expected) { + hal.console->println_P( + PSTR("LSM303D _read_data_transaction_accel: _reg7_expected unexpected")); + // reset(); + return; + } + + struct { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + } raw_mag_report; + + /* fetch data from the sensor */ + memset(&raw_mag_report, 0, sizeof(raw_mag_report)); + raw_mag_report.cmd = ADDR_STATUS_M | DIR_READ | ADDR_INCREMENT; + _spi->transaction((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report)); + + _mag_sum.x = raw_mag_report.x; + _mag_sum.y = raw_mag_report.y; + _mag_sum.z = raw_mag_report.z; +} + +void AP_InertialSensor_LSM303D::_read_data_transaction() { + + _read_data_transaction_accel(); + _read_data_transaction_mag(); + _sum_count++; + + if (_sum_count == 0) { + // rollover - v unlikely + _accel_sum.zero(); + _mag_sum.zero(); + } +} + +uint8_t AP_InertialSensor_LSM303D::_register_read( uint8_t reg ) +{ + 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); + + return rx[1]; +} + +void AP_InertialSensor_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); +} + +/* + useful when debugging SPI bus errors + */ +void AP_InertialSensor_LSM303D::_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_P(PSTR("Values doesn't match; written: %02x; read: %02x "), val, readed); + } +#if LSM303D_DEBUG + hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed); +#endif +} + +void AP_InertialSensor_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); +} + + +/* + set the DLPF filter frequency. Assumes caller has taken semaphore + TODO needs to be changed according to LSM303D needs + */ +// void AP_InertialSensor_LSM303D::_set_filter_register(uint8_t filter_hz, uint8_t default_filter) +// { +// uint8_t filter = default_filter; +// // choose filtering frequency +// switch (filter_hz) { +// case 5: +// filter = BITS_DLPF_CFG_5HZ; +// break; +// case 10: +// filter = BITS_DLPF_CFG_10HZ; +// break; +// case 20: +// filter = BITS_DLPF_CFG_20HZ; +// break; +// case 42: +// filter = BITS_DLPF_CFG_42HZ; +// break; +// case 98: +// filter = BITS_DLPF_CFG_98HZ; +// break; +// } + +// if (filter != 0) { +// _last_filter_hz = filter_hz; +// _register_write(MPUREG_CONFIG, filter); +// } +// } + +void AP_InertialSensor_LSM303D::disable_i2c(void) +{ + 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)); +} + +uint8_t AP_InertialSensor_LSM303D::accel_set_range(uint8_t max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_FULL_SCALE_BITS_A; + float new_scale_g_digit = 0.0f; + + if (max_g == 0) + max_g = 16; + + if (max_g <= 2) { + _accel_range_m_s2 = 2.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_2G_A; + new_scale_g_digit = 0.061e-3f; + + } else if (max_g <= 4) { + _accel_range_m_s2 = 4.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_4G_A; + new_scale_g_digit = 0.122e-3f; + + } else if (max_g <= 6) { + _accel_range_m_s2 = 6.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_6G_A; + new_scale_g_digit = 0.183e-3f; + + } else if (max_g <= 8) { + _accel_range_m_s2 = 8.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_8G_A; + new_scale_g_digit = 0.244e-3f; + + } else if (max_g <= 16) { + _accel_range_m_s2 = 16.0f*LSM303D_ONE_G; + setbits |= REG2_FULL_SCALE_16G_A; + new_scale_g_digit = 0.732e-3f; + + } else { + return -1; + } + + _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + _register_modify(ADDR_CTRL_REG2, clearbits, setbits); + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::accel_set_samplerate(uint8_t frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG1_RATE_BITS_A; + + if (frequency == 0) + frequency = 1600; + + if (frequency <= 100) { + setbits |= REG1_RATE_100HZ_A; + _accel_samplerate = 100; + + } else if (frequency <= 200) { + setbits |= REG1_RATE_200HZ_A; + _accel_samplerate = 200; + + } else if (frequency <= 400) { + setbits |= REG1_RATE_400HZ_A; + _accel_samplerate = 400; + + } else if (frequency <= 800) { + setbits |= REG1_RATE_800HZ_A; + _accel_samplerate = 800; + + } else if (frequency <= 1600) { + setbits |= REG1_RATE_1600HZ_A; + _accel_samplerate = 1600; + + } else { + return -1; + } + + _register_modify(ADDR_CTRL_REG1, clearbits, setbits); + _reg1_expected = (_reg1_expected & ~clearbits) | setbits; + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::accel_set_onchip_lowpass_filter_bandwidth(uint8_t bandwidth) +{ + uint8_t setbits = 0; + uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A; + + if (bandwidth == 0) + bandwidth = 773; + + if (bandwidth <= 50) { + setbits |= REG2_AA_FILTER_BW_50HZ_A; + _accel_onchip_filter_bandwith = 50; + + } else if (bandwidth <= 194) { + setbits |= REG2_AA_FILTER_BW_194HZ_A; + _accel_onchip_filter_bandwith = 194; + + } else if (bandwidth <= 362) { + setbits |= REG2_AA_FILTER_BW_362HZ_A; + _accel_onchip_filter_bandwith = 362; + + } else if (bandwidth <= 773) { + setbits |= REG2_AA_FILTER_BW_773HZ_A; + _accel_onchip_filter_bandwith = 773; + + } else { + return -1; + } + + _register_modify(ADDR_CTRL_REG2, clearbits, setbits); + return 0; +} + +uint8_t AP_InertialSensor_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.080e-3f; + + } else if (max_ga <= 4) { + _mag_range_ga = 4; + setbits |= REG6_FULL_SCALE_4GA_M; + new_scale_ga_digit = 0.160e-3f; + + } else if (max_ga <= 8) { + _mag_range_ga = 8; + setbits |= REG6_FULL_SCALE_8GA_M; + new_scale_ga_digit = 0.320e-3f; + + } else if (max_ga <= 12) { + _mag_range_ga = 12; + setbits |= REG6_FULL_SCALE_12GA_M; + new_scale_ga_digit = 0.479e-3f; + + } else { + return -1; + } + + _mag_range_scale = new_scale_ga_digit; + _register_modify(ADDR_CTRL_REG6, clearbits, setbits); + return 0; +} + +uint8_t AP_InertialSensor_LSM303D::mag_set_samplerate(uint8_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 -1; + } + + _register_modify(ADDR_CTRL_REG5, clearbits, setbits); + return 0; +} + +bool AP_InertialSensor_LSM303D::_hardware_init(Sample_rate sample_rate) +{ + if (!_spi_sem->take(100)) { + hal.scheduler->panic(PSTR("LSM303D: Unable to get semaphore")); + } + + // initially run the bus at low speed + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + // ensure the chip doesn't interpret any other bus traffic as I2C + disable_i2c(); + + + /* enable accel*/ + _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; + _register_write(ADDR_CTRL_REG1, _reg1_expected); + + /* 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_REG3, 0x04); // DRDY on ACCEL on INT1 + _register_write(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 + + accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); + + // Hardware filtering + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); + mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); + + // TODO: Software filtering + // accel_set_driver_lowpass_filter((float)LSM303D_ACCEL_DEFAULT_RATE, (float)LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + + // uint8_t default_filter; + + // // sample rate and filtering + // // to minimise the effects of aliasing we choose a filter + // // that is less than half of the sample rate + // switch (sample_rate) { + // case RATE_50HZ: + // // this is used for plane and rover, where noise resistance is + // // more important than update rate. Tests on an aerobatic plane + // // show that 10Hz is fine, and makes it very noise resistant + // default_filter = BITS_DLPF_CFG_10HZ; + // _sample_shift = 2; + // break; + // case RATE_100HZ: + // default_filter = BITS_DLPF_CFG_20HZ; + // _sample_shift = 1; + // break; + // case RATE_200HZ: + // default: + // default_filter = BITS_DLPF_CFG_20HZ; + // _sample_shift = 0; + // break; + // } + // _set_filter_register(_LSM303D_filter, default_filter); + + // 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(); + + return true; +} + +// return true if a sample is available +bool AP_InertialSensor_LSM303D::_sample_available() +{ + _poll_data(); + // return (_sum_count >> _sample_shift) > 0; + return (_sum_count) > 0; +} + + +// TODO fix dump registers +#if LSM303D_DEBUG +// dump all config registers - used for debug +void AP_InertialSensor_LSM303D::_dump_registers(void) +{ + hal.console->println_P(PSTR("LSM303D registers")); + if (_spi_sem->take(100)) { + for (uint8_t reg=ADDR_WHO_AM_I; reg<=56; reg++) { // 0x38 = 56 + uint8_t v = _register_read(reg); + hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); + if ((reg - (ADDR_WHO_AM_I-1)) % 16 == 0) { + hal.console->println(); + } + } + hal.console->println(); + _spi_sem->give(); + } +} +#endif + + +// get_delta_time returns the time period in seconds overwhich the sensor data was collected +float AP_InertialSensor_LSM303D::get_delta_time() const +{ + // the sensor runs at 200Hz + return 0.005 * _num_samples; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h new file mode 100644 index 0000000000..e2b7b08c88 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h @@ -0,0 +1,107 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#ifndef __AP_INERTIAL_SENSOR_LSM303D_H__ +#define __AP_INERTIAL_SENSOR_LSM303D_H__ + +#include +#include +#include +#include +#include "AP_InertialSensor.h" + +// enable debug to see a register dump on startup +#define LSM303D_DEBUG 0 + +class AP_InertialSensor_LSM303D: public AP_InertialSensor +{ +public: + + AP_InertialSensor_LSM303D(); + + /* Concrete implementation of AP_InertialSensor functions: */ + bool update(); + + // wait for a sample to be available, with timeout in milliseconds + bool wait_for_sample(uint16_t timeout_ms); + + // get_delta_time returns the time period in seconds overwhich the sensor data was collected + float get_delta_time() const; + + uint16_t error_count(void) const { return _error_count; } + bool healthy(void) const { return _error_count <= 4; } + bool get_accel_health(uint8_t instance) const { return healthy(); } + +protected: + uint16_t _init_sensor( Sample_rate sample_rate ); + +private: + AP_HAL::DigitalSource *_drdy_pin_x; + AP_HAL::DigitalSource *_drdy_pin_m; + uint8_t _accel_range_m_s2; + float _accel_range_scale; + uint8_t _accel_samplerate; + uint8_t _accel_onchip_filter_bandwith; + uint8_t _mag_range_ga; + float _mag_range_scale; + uint8_t _mag_samplerate; + + // expceted values of reg1 and reg7 to catch in-flight + // brownouts of the sensor + uint8_t _reg1_expected; + uint8_t _reg7_expected; + + bool _sample_available(); + void _read_data_transaction(); + void _read_data_transaction_accel(); + void _read_data_transaction_mag(); + bool _data_ready(); + void _poll_data(void); + 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 _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits); + bool _hardware_init(Sample_rate sample_rate); + void disable_i2c(void); + uint8_t accel_set_range(uint8_t max_g); + uint8_t accel_set_samplerate(uint8_t frequency); + uint8_t accel_set_onchip_lowpass_filter_bandwidth(uint8_t bandwidth); + uint8_t mag_set_range(uint8_t max_ga); + uint8_t mag_set_samplerate(uint8_t frequency); + + AP_HAL::SPIDeviceDriver *_spi; + AP_HAL::Semaphore *_spi_sem; + + uint16_t _num_samples; + float _accel_scale; + float _mag_scale; + + uint32_t _last_sample_time_micros; + + // ensure we can't initialise twice + bool _initialised; + int16_t _LSM303D_product_id; + + // how many hardware samples before we report a sample to the caller + uint8_t _sample_shift; + + // support for updating filter at runtime + uint8_t _last_filter_hz; + + void _set_filter_register(uint8_t filter_hz, uint8_t default_filter); + + uint16_t _error_count; + + // accumulation in timer - must be read with timer disabled + // the sum of the values since last read + Vector3l _accel_sum; + Vector3l _mag_sum; + volatile int16_t _sum_count; + +public: + +#if LSM303D_DEBUG + void _dump_registers(void); +#endif +}; + +#endif // __AP_INERTIAL_SENSOR_LSM303D_H__