2014-07-24 20:19:25 -03:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
2014-10-14 01:48:33 -03:00
|
|
|
#if NOT_YET
|
|
|
|
|
2014-07-24 20:19:25 -03:00
|
|
|
/****************************************************************************
|
|
|
|
*
|
|
|
|
* Coded by Víctor Mayoral Vilches <v.mayoralv@gmail.com> using
|
|
|
|
* lsm3030d.cpp <https://github.com/diydrones/PX4Firmware> 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 <AP_HAL.h>
|
|
|
|
#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);
|
2014-08-18 10:11:35 -03:00
|
|
|
hal.scheduler->panic(PSTR("LSM303D: bad WHOAMI"));
|
2014-07-24 20:19:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
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;
|
|
|
|
}
|
2014-08-18 10:11:35 -03:00
|
|
|
// TODO: read status register
|
|
|
|
return false;
|
2014-07-24 20:19:25 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
|
|
* 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;
|
|
|
|
}
|
2014-10-14 01:48:33 -03:00
|
|
|
#endif
|