/// -*- 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(PSTR("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; } // TODO: read status register return false; } /** * 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; }