From 42eb73a1d12ac4b28ef90911c56cdb1734104385 Mon Sep 17 00:00:00 2001 From: Gustavo Jose de Sousa Date: Wed, 13 May 2015 19:39:03 -0300 Subject: [PATCH] AP_InertialSensor: add LSM9DS0 backend MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This adds the backend driver for LSM9DS0. This implementation is based on the legacy driver coded by Víctor Mayoral Vilches (under folder LSM9DS0) and makes some necessary adaptations and fixes in order to work properly. The legacy driver folder was removed. --- libraries/AP_HAL/AP_HAL_Boards.h | 3 +- .../AP_InertialSensor/AP_InertialSensor.cpp | 2 + .../AP_InertialSensor/AP_InertialSensor.h | 1 + .../AP_InertialSensor_LSM9DS0.cpp | 669 ++++++++++++++ .../AP_InertialSensor_LSM9DS0.h | 107 +++ .../LSM9DS0/AP_InertialSensor_LSM9DS0.cpp | 816 ------------------ .../LSM9DS0/AP_InertialSensor_LSM9DS0.h | 210 ----- 7 files changed, 781 insertions(+), 1027 deletions(-) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h delete mode 100644 libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.cpp delete mode 100644 libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.h diff --git a/libraries/AP_HAL/AP_HAL_Boards.h b/libraries/AP_HAL/AP_HAL_Boards.h index 0f3aa2e7f1..00977bf70b 100644 --- a/libraries/AP_HAL/AP_HAL_Boards.h +++ b/libraries/AP_HAL/AP_HAL_Boards.h @@ -63,7 +63,8 @@ #define HAL_INS_L3G4200D 6 #define HAL_INS_VRBRAIN 7 #define HAL_INS_MPU9250 8 -#define HAL_INS_L3GD20 9 +#define HAL_INS_L3GD20 9 +#define HAL_INS_LSM9DS0 10 // barometer driver types #define HAL_BARO_BMP085 1 diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 1dfbd65086..9d87bf24d4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -416,6 +416,8 @@ AP_InertialSensor::_detect_backends(void) _add_backend(AP_InertialSensor_MPU9250::detect); #elif HAL_INS_DEFAULT == HAL_INS_FLYMAPLE _add_backend(AP_InertialSensor_Flymaple::detect); +#elif HAL_INS_DEFAULT == HAL_INS_LSM9DS0 + _add_backend(AP_InertialSensor_LSM9DS0::detect); #else #error Unrecognised HAL_INS_TYPE setting #endif diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h index 0d3731da66..c0a14cdd51 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor.h @@ -372,6 +372,7 @@ private: #include "AP_InertialSensor_L3G4200D.h" #include "AP_InertialSensor_Flymaple.h" #include "AP_InertialSensor_MPU9150.h" +#include "AP_InertialSensor_LSM9DS0.h" #include "AP_InertialSensor_HIL.h" #include "AP_InertialSensor_UserInteract_Stream.h" #include "AP_InertialSensor_UserInteract_MAVLink.h" diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp new file mode 100644 index 0000000000..fb36765e03 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.cpp @@ -0,0 +1,669 @@ +/* + 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 . + + -- Adapted from Victor Mayoral Vilches's legacy driver under folder LSM9DS0 +*/ + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "AP_InertialSensor_LSM9DS0.h" +#include "../AP_HAL_Linux/GPIO.h" + +extern const AP_HAL::HAL& hal; + +#define LSM9DS0_G_WHOAMI 0xD4 +#define LSM9DS0_XM_WHOAMI 0x49 + +/* + * If data-ready GPIO pins are not defined, the fallback approach used is to + * check if there's new data ready by reading the status register. It is + * *strongly* recommended to use data-ready GPIO pins for performance reasons. + */ +#ifndef DRDY_GPIO_PIN_A + #define DRDY_GPIO_PIN_A 0 +#endif +#ifndef DRDY_GPIO_PIN_G + #define DRDY_GPIO_PIN_G 0 +#endif + +//////////////////////////// +// LSM9DS0 Gyro Registers // +//////////////////////////// +#define WHO_AM_I_G 0x0F +#define CTRL_REG1_G 0x20 +#define CTRL_REG2_G 0x21 +#define CTRL_REG3_G 0x22 +#define CTRL_REG4_G 0x23 +#define CTRL_REG5_G 0x24 +#define REFERENCE_G 0x25 +#define STATUS_REG_G 0x27 +#define OUT_X_L_G 0x28 +#define OUT_X_H_G 0x29 +#define OUT_Y_L_G 0x2A +#define OUT_Y_H_G 0x2B +#define OUT_Z_L_G 0x2C +#define OUT_Z_H_G 0x2D +#define FIFO_CTRL_REG_G 0x2E +#define FIFO_SRC_REG_G 0x2F +#define INT1_CFG_G 0x30 +#define INT1_SRC_G 0x31 +#define INT1_THS_XH_G 0x32 +#define INT1_THS_XL_G 0x33 +#define INT1_THS_YH_G 0x34 +#define INT1_THS_YL_G 0x35 +#define INT1_THS_ZH_G 0x36 +#define INT1_THS_ZL_G 0x37 +#define INT1_DURATION_G 0x38 + +////////////////////////////////////////// +// LSM9DS0 Accel/Magneto (XM) Registers // +////////////////////////////////////////// +#define OUT_TEMP_L_XM 0x05 +#define OUT_TEMP_H_XM 0x06 +#define STATUS_REG_M 0x07 +#define OUT_X_L_M 0x08 +#define OUT_X_H_M 0x09 +#define OUT_Y_L_M 0x0A +#define OUT_Y_H_M 0x0B +#define OUT_Z_L_M 0x0C +#define OUT_Z_H_M 0x0D +#define WHO_AM_I_XM 0x0F +#define INT_CTRL_REG_M 0x12 +#define INT_SRC_REG_M 0x13 +#define INT_THS_L_M 0x14 +#define INT_THS_H_M 0x15 +#define OFFSET_X_L_M 0x16 +#define OFFSET_X_H_M 0x17 +#define OFFSET_Y_L_M 0x18 +#define OFFSET_Y_H_M 0x19 +#define OFFSET_Z_L_M 0x1A +#define OFFSET_Z_H_M 0x1B +#define REFERENCE_X 0x1C +#define REFERENCE_Y 0x1D +#define REFERENCE_Z 0x1E +#define CTRL_REG0_XM 0x1F +#define CTRL_REG1_XM 0x20 +#define CTRL_REG2_XM 0x21 +#define CTRL_REG3_XM 0x22 +#define CTRL_REG4_XM 0x23 +#define CTRL_REG5_XM 0x24 +#define CTRL_REG6_XM 0x25 +#define CTRL_REG7_XM 0x26 +#define STATUS_REG_A 0x27 +#define OUT_X_L_A 0x28 +#define OUT_X_H_A 0x29 +#define OUT_Y_L_A 0x2A +#define OUT_Y_H_A 0x2B +#define OUT_Z_L_A 0x2C +#define OUT_Z_H_A 0x2D +#define FIFO_CTRL_REG 0x2E +#define FIFO_SRC_REG 0x2F +#define INT_GEN_1_REG 0x30 +#define INT_GEN_1_SRC 0x31 +#define INT_GEN_1_THS 0x32 +#define INT_GEN_1_DURATION 0x33 +#define INT_GEN_2_REG 0x34 +#define INT_GEN_2_SRC 0x35 +#define INT_GEN_2_THS 0x36 +#define INT_GEN_2_DURATION 0x37 +#define CLICK_CFG 0x38 +#define CLICK_SRC 0x39 +#define CLICK_THS 0x3A +#define TIME_LIMIT 0x3B +#define TIME_LATENCY 0x3C +#define TIME_WINDOW 0x3D +#define ACT_THS 0x3E +#define ACT_DUR 0x3F + +AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu): + AP_InertialSensor_Backend(imu), + _drdy_pin_a(NULL), + _drdy_pin_g(NULL), + _last_accel_filter_hz(-1), + _last_gyro_filter_hz(-1), + _accel_filter(800, 15), + _gyro_filter(760, 15), + _gyro_sample_available(false), + _accel_sample_available(false) +{ + _product_id = AP_PRODUCT_ID_NONE; +} + +AP_InertialSensor_Backend *AP_InertialSensor_LSM9DS0::detect(AP_InertialSensor &_imu) +{ + AP_InertialSensor_LSM9DS0 *sensor = new AP_InertialSensor_LSM9DS0(_imu); + + if (sensor == NULL) { + return NULL; + } + + if (!sensor->_init_sensor()) { + delete sensor; + return NULL; + } + + return sensor; +} + +bool AP_InertialSensor_LSM9DS0::_init_sensor() +{ + _gyro_spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_G); + _accel_spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_AM); + _spi_sem = _gyro_spi->get_semaphore(); /* same semaphore for both */ + +#if DRDY_GPIO_PIN_A != 0 + _drdy_pin_a = hal.gpio->channel(DRDY_GPIO_PIN_A); + if (_drdy_pin_a == NULL) { + hal.scheduler->panic("LSM9DS0: null accel data-ready GPIO channel\n"); + } +#endif + +#if DRDY_GPIO_PIN_G != 0 + _drdy_pin_g = hal.gpio->channel(DRDY_GPIO_PIN_G); + if (_drdy_pin_g == NULL) { + hal.scheduler->panic("LSM9DS0: null gyro data-ready GPIO channel\n"); + } +#endif + + hal.scheduler->suspend_timer_procs(); + + uint8_t whoami; + + bool whoami_ok = true; + + whoami = _register_read_g(WHO_AM_I_G); + if (whoami != LSM9DS0_G_WHOAMI) { + hal.console->printf("LSM9DS0: unexpected gyro WHOAMI 0x%x\n", (unsigned)whoami); + whoami_ok = false; + } + + whoami = _register_read_xm(WHO_AM_I_XM); + if (whoami != LSM9DS0_XM_WHOAMI) { + hal.console->printf("LSM9DS0: unexpected acc/mag WHOAMI 0x%x\n", (unsigned)whoami); + whoami_ok = false; + } + + if (!whoami_ok) return false; + + uint8_t tries = 0; + bool a_ready = false; + bool g_ready = false; + do { + bool success = _hardware_init(); + if (success) { + hal.scheduler->delay(10); + if (!_spi_sem->take(100)) { + hal.console->printf("LSM9DS0: Unable to get semaphore\n"); + return false; + } + if (!a_ready) { + a_ready = _accel_data_ready(); + } + if (!g_ready) { + g_ready = _gyro_data_ready(); + } + if (a_ready && g_ready) { + _spi_sem->give(); + break; + } else { + hal.console->printf("LSM9DS0 startup failed: no data ready\n"); + } + _spi_sem->give(); + } + if (tries++ > 5) { + hal.console->printf("failed to boot LSM9DS0 5 times\n"); + return false; + } + } while (1); + + hal.scheduler->resume_timer_procs(); + + _gyro_instance = _imu.register_gyro(); + _accel_instance = _imu.register_accel(); + +#if LSM9DS0_DEBUG + _dump_registers(); +#endif + + /* start the timer process to read samples */ + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void)); + + return true; +} + +bool AP_InertialSensor_LSM9DS0::_hardware_init() +{ + if (!_spi_sem->take(100)) { + hal.console->printf("LSM9DS0: Unable to get semaphore\n"); + return false; + } + + _gyro_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + _accel_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + + _gyro_init(); + _accel_init(); + + _gyro_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + _accel_spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + + _spi_sem->give(); + + return true; +} + +uint8_t AP_InertialSensor_LSM9DS0::_register_read_xm( uint8_t reg ) +{ + uint8_t addr = reg | 0x80; /* set read bit */ + + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = addr; + tx[1] = 0; + _accel_spi->transaction(tx, rx, 2); + + return rx[1]; +} + +uint8_t AP_InertialSensor_LSM9DS0::_register_read_g( uint8_t reg ) +{ + uint8_t addr = reg | 0x80; /* set read bit */ + + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = addr; + tx[1] = 0; + _gyro_spi->transaction(tx, rx, 2); + + return rx[1]; +} + + +void AP_InertialSensor_LSM9DS0::_register_write_xm(uint8_t reg, uint8_t val) +{ + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = reg; + tx[1] = val; + _accel_spi->transaction(tx, rx, 2); +} + +void AP_InertialSensor_LSM9DS0::_register_write_g(uint8_t reg, uint8_t val) +{ + uint8_t tx[2]; + uint8_t rx[2]; + + tx[0] = reg; + tx[1] = val; + _gyro_spi->transaction(tx, rx, 2); +} + +void AP_InertialSensor_LSM9DS0::_gyro_init() +{ + /* + * CTRL_REG1_G sets output data rate, bandwidth, power-down and enables + * + * Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen + * DR[1:0] - Output data rate selection + * 00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz + * BW[1:0] - Bandwidth selection (sets cutoff frequency) + * Value depends on ODR. See datasheet table 21. + * PD - Power down enable (0=power down mode, 1=normal or sleep mode) + * Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) + * + * Data rate of 760Hz, cutoff of 50Hz, Normal mode, enable all axes + */ + _register_write_g(CTRL_REG1_G, 0xEF); + hal.scheduler->delay(1); + + /* + * CTRL_REG2_G sets up the HPF + * + * Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0 + * HPM[1:0] - High pass filter mode selection + * 00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering, + * 10=normal, 11=autoreset on interrupt + * HPCF[3:0] - High pass filter cutoff frequency + * Value depends on data rate. See datasheet table 26. + * + * Normal mode, high cutoff frequency + */ + _register_write_g(CTRL_REG2_G, 0x00); + hal.scheduler->delay(1); + + /* + * CTRL_REG3_G sets up interrupt and DRDY_G pins + * + * Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY + * I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable) + * I1_BOOT - Boot status available on INT_G (0=disable, 1=enable) + * H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low) + * PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain) + * I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable) + * I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable) + * I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable) + * I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) + * + * Gyro data ready on DRDY_G + */ + _register_write_g(CTRL_REG3_G, 0x08); + hal.scheduler->delay(1); + + /* + * CTRL_REG4_G sets the scale, update mode + * + * Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM + * BDU - Block data update (0=continuous, 1=output not updated until read + * BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add) + * FS[1:0] - Full-scale selection + * 00=245dps, 01=500dps, 10=2000dps, 11=2000dps + * ST[1:0] - Self-test enable + * 00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+) + * SIM - SPI serial interface mode select + * 0=4 wire, 1=3 wire + * + * BUD, set scale to 2000 dps + */ + _register_write_g(CTRL_REG4_G, 0xB0); + _set_gyro_scale(G_SCALE_2000DPS); + hal.scheduler->delay(1); + + /* + * CTRL_REG5_G sets up the FIFO, HPF, and INT1 + * + * Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0 + * BOOT - Reboot memory content (0=normal, 1=reboot) + * FIFO_EN - FIFO enable (0=disable, 1=enable) + * HPen - HPF enable (0=disable, 1=enable) + * INT1_Sel[1:0] - Int 1 selection configuration + * Out_Sel[1:0] - Out selection configuration + */ + _register_write_g(CTRL_REG5_G, 0x00); + hal.scheduler->delay(1); +} + +void AP_InertialSensor_LSM9DS0::_accel_init() +{ + /* + * CTRL_REG0_XM (0x1F) (Default value: 0x00) + * + * Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2 + * BOOT - Reboot memory content (0: normal, 1: reboot) + * FIFO_EN - Fifo enable (0: disable, 1: enable) + * WTM_EN - FIFO watermark enable (0: disable, 1: enable) + * HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled) + * HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled) + * HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled) + */ + _register_write_xm(CTRL_REG0_XM, 0x00); + hal.scheduler->delay(1); + + /* + * CTRL_REG1_XM (0x20) (Default value: 0x07) + * + * Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN + * AODR[3:0] - select the acceleration data rate: + * 0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, + * 0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz, + * 1001=800Hz, 1010=1600Hz, (remaining combinations undefined). + * BDU - block data update for accel AND mag + * 0: Continuous update + * 1: Output registers aren't updated until MSB and LSB have been read. + * AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled. + * 0: Axis disabled, 1: Axis enabled + * + * 800Hz data rate, BDU enabled, x,y,z all enabled + */ + _register_write_xm(CTRL_REG1_XM, 0x9F); + hal.scheduler->delay(1); + + /* + * CTRL_REG2_XM (0x21) (Default value: 0x00) + * + * Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM + * ABW[1:0] - Accelerometer anti-alias filter bandwidth + * 00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz + * AFS[2:0] - Accel full-scale selection + * 000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g + * AST[1:0] - Accel self-test enable + * 00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed + * SIM - SPI mode selection + * 0=4-wire, 1=3-wire + * + * Set filter bandwidth to 50Hz and scale to 16g + */ + _register_write_xm(CTRL_REG2_XM, 0xE0); + _set_accel_scale(A_SCALE_16G); + hal.scheduler->delay(1); + + /* + * CTRL_REG3_XM is used to set interrupt generators on INT1_XM + * + * Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY + * + * Accel data ready on INT1 + */ + _register_write_xm(CTRL_REG3_XM, 0x04); + hal.scheduler->delay(1); +} + +void AP_InertialSensor_LSM9DS0::_set_gyro_scale(gyro_scale scale) +{ + /* scales values from datasheet in mdps/digit */ + switch (scale) { + case G_SCALE_245DPS: + _gyro_scale = 8.75; + break; + case G_SCALE_500DPS: + _gyro_scale = 17.50; + break; + case G_SCALE_2000DPS: + _gyro_scale = 70; + break; + } + + _gyro_scale /= 1000; /* convert mdps/digit to dps/digit */ + _gyro_scale *= DEG_TO_RAD; /* convert dps/digit to (rad/s)/digit */ +} + +void AP_InertialSensor_LSM9DS0::_set_accel_scale(accel_scale scale) +{ + /* + * Possible accelerometer scales (and their register bit settings) are: + * 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an + * algorithm to calculate g/(ADC tick) based on that 3-bit value: + */ + _accel_scale = (((float) scale + 1.0f) * 2.0f) / 32768.0f; + if (scale == A_SCALE_16G) { + _accel_scale = 0.000732; /* the datasheet shows an exception for +-16G */ + } + _accel_scale *= GRAVITY_MSS; /* convert to G/LSB to (m/s/s)/LSB */ +} + +/** + * Timer process to poll for new data from the LSM9DS0. + */ +void AP_InertialSensor_LSM9DS0::_poll_data() +{ + 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 (_gyro_data_ready()) { + _read_data_transaction_g(); + } + + if (_accel_data_ready()) { + _read_data_transaction_a(); + } + + _spi_sem->give(); +} + +#if DRDY_GPIO_PIN_A != 0 +bool AP_InertialSensor_LSM9DS0::_accel_data_ready() +{ + return _drdy_pin_a->read() != 0; +} +#else +bool AP_InertialSensor_LSM9DS0::_accel_data_ready() +{ + uint8_t status = _register_read_xm(STATUS_REG_A); + return status & STATUS_REG_A_ZYXADA; +} +#endif /* DRDY_GPIO_PIN_A != 0 */ + +#if DRDY_GPIO_PIN_G != 0 +bool AP_InertialSensor_LSM9DS0::_gyro_data_ready() +{ + return _drdy_pin_g->read() != 0; +} +#else +bool AP_InertialSensor_LSM9DS0::_gyro_data_ready() +{ + uint8_t status = _register_read_xm(STATUS_REG_G); + return status & STATUS_REG_G_ZYXDA; +} +#endif /* DRDY_GPIO_PIN_G != 0 */ + +void AP_InertialSensor_LSM9DS0::_accel_raw_data(struct sensor_raw_data *raw_data) +{ + struct __attribute__((packed)) { + uint8_t reg; + struct sensor_raw_data data; + } tx = {.reg = OUT_X_L_A | 0xC0, .data = {}}, rx; + + _accel_spi->transaction((uint8_t *)&tx, (uint8_t *)&rx, 7); + *raw_data = rx.data; +} + +void AP_InertialSensor_LSM9DS0::_gyro_raw_data(struct sensor_raw_data *raw_data) +{ + struct __attribute__((packed)) { + uint8_t reg; + struct sensor_raw_data data; + } tx = {.reg = OUT_X_L_G | 0xC0, .data = {}}, rx; + + _gyro_spi->transaction((uint8_t *)&tx, (uint8_t *)&rx, 7); + *raw_data = rx.data; +} + +void AP_InertialSensor_LSM9DS0::_read_data_transaction_a() +{ + struct sensor_raw_data raw_data; + _accel_raw_data(&raw_data); + + Vector3f accel_data(raw_data.x, -raw_data.y, -raw_data.z); + _accel_filtered = _accel_filter.apply(accel_data); + _accel_sample_available = true; +} + +/* + read from the data registers and update filtered data + */ +void AP_InertialSensor_LSM9DS0::_read_data_transaction_g() +{ + struct sensor_raw_data raw_data; + _gyro_raw_data(&raw_data); + + Vector3f gyro_data(raw_data.x, -raw_data.y, -raw_data.z); + _gyro_filtered = _gyro_filter.apply(gyro_data); + _gyro_sample_available = true; +} + +bool AP_InertialSensor_LSM9DS0::update() +{ + Vector3f gyro = _gyro_filtered; + Vector3f accel = _accel_filtered; + + _accel_sample_available = false; + _gyro_sample_available = false; + + accel *= _accel_scale; + gyro *= _gyro_scale; + + _publish_gyro(_gyro_instance, gyro); + _publish_accel(_accel_instance, accel); + + if (_last_accel_filter_hz != _accel_filter_cutoff()) { + _set_accel_filter(_accel_filter_cutoff()); + _last_accel_filter_hz = _accel_filter_cutoff(); + } + + if (_last_gyro_filter_hz != _gyro_filter_cutoff()) { + _set_gyro_filter(_gyro_filter_cutoff()); + _last_gyro_filter_hz = _gyro_filter_cutoff(); + } + + return true; +} + +/* + set the accel filter frequency + */ +void AP_InertialSensor_LSM9DS0::_set_accel_filter(uint8_t filter_hz) +{ + _accel_filter.set_cutoff_frequency(800, filter_hz); +} + +/* + set the gyro filter frequency + */ +void AP_InertialSensor_LSM9DS0::_set_gyro_filter(uint8_t filter_hz) +{ + _gyro_filter.set_cutoff_frequency(760, filter_hz); +} + +#if LSM9DS0_DEBUG +/* dump all config registers - used for debug */ +void AP_InertialSensor_LSM9DS0::_dump_registers(void) +{ + hal.console->println_P(PSTR("LSM9DS0 registers:")); + hal.console->println_P(PSTR("Gyroscope registers:")); + const uint8_t first = OUT_TEMP_L_XM; + const uint8_t last = ACT_DUR; + for (uint8_t reg=first; reg<=last; reg++) { + uint8_t v = _register_read_g(reg); + hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); + if ((reg - (first-1)) % 16 == 0) { + hal.console->println(); + } + } + hal.console->println(); + + hal.console->println_P(PSTR("Accelerometer and Magnetometers registers:")); + for (uint8_t reg=first; reg<=last; reg++) { + uint8_t v = _register_read_xm(reg); + hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); + if ((reg - (first-1)) % 16 == 0) { + hal.console->println(); + } + } + hal.console->println(); + +} +#endif + +#endif /* CONFIG_HAL_BOARD == HAL_BOARD_LINUX */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h new file mode 100644 index 0000000000..075e2e71e6 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM9DS0.h @@ -0,0 +1,107 @@ + +#ifndef __AP_INERTIAL_SENSOR_LSM9DS0_H__ +#define __AP_INERTIAL_SENSOR_LSM9DS0_H__ + +#define LSM9DS0_DEBUG 1 + +#include +#include "AP_InertialSensor.h" + +class AP_InertialSensor_LSM9DS0 : public AP_InertialSensor_Backend +{ +public: + + enum gyro_scale + { + G_SCALE_245DPS = 0, + G_SCALE_500DPS, + G_SCALE_2000DPS, + }; + + enum accel_scale + { + A_SCALE_2G = 0, + A_SCALE_4G, + A_SCALE_6G, + A_SCALE_8G, + A_SCALE_16G + }; + + AP_InertialSensor_LSM9DS0(AP_InertialSensor &imu); + + bool update(); + + bool gyro_sample_available() { return _gyro_sample_available; }; + + bool accel_sample_available() { return _accel_sample_available; }; + + static AP_InertialSensor_Backend *detect(AP_InertialSensor &imu); + +private: + struct PACKED sensor_raw_data { + int16_t x; + int16_t y; + int16_t z; + }; + + AP_HAL::SPIDeviceDriver *_accel_spi; + AP_HAL::SPIDeviceDriver *_gyro_spi; + AP_HAL::Semaphore *_spi_sem; + + AP_HAL::DigitalSource *_drdy_pin_a; + AP_HAL::DigitalSource *_drdy_pin_g; + + bool _gyro_sample_available; + bool _accel_sample_available; + + bool _accel_data_ready(); + bool _gyro_data_ready(); + + void _poll_data(); + + bool _init_sensor(); + bool _hardware_init(); + + uint8_t _gyro_instance; + uint8_t _accel_instance; + + void _gyro_init(); + void _accel_init(); + + float _gyro_scale, _accel_scale; + void _set_gyro_scale(gyro_scale scale); + void _set_accel_scale(accel_scale scale); + + uint8_t _register_read_xm( uint8_t reg ); + uint8_t _register_read_g( uint8_t reg ); + + void _register_write_xm( uint8_t reg, uint8_t val ); + void _register_write_g( uint8_t reg, uint8_t val ); + + void _accel_raw_data(struct sensor_raw_data *raw_data); + void _gyro_raw_data(struct sensor_raw_data *raw_data); + + void _read_data_transaction_a(); + void _read_data_transaction_g(); + + /* support for updating filter at runtime */ + int16_t _last_gyro_filter_hz; + int16_t _last_accel_filter_hz; + + /* change the filter frequency */ + void _set_accel_filter(uint8_t filter_hz); + void _set_gyro_filter(uint8_t filter_hz); + + Vector3f _accel_filtered; + Vector3f _gyro_filtered; + + /* Low Pass filters for gyro and accel */ + LowPassFilter2pVector3f _accel_filter; + LowPassFilter2pVector3f _gyro_filter; + +#if LSM9DS0_DEBUG + void _dump_registers(); +#endif +}; + +#endif /* __AP_INERTIAL_SENSOR_LSM9DS0_H__ */ diff --git a/libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.cpp b/libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.cpp deleted file mode 100644 index 3ae689ed0e..0000000000 --- a/libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.cpp +++ /dev/null @@ -1,816 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -/* - 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 . - - -- Coded by Victor Mayoral Vilches -- - the code reuses partially the Sparkfun library - from https://github.com/sparkfun/LSM9DS0_Breakout/tree/master/Libraries/Arduino/SFE_LSM9DS0 - -*/ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX -#include "AP_InertialSensor_LSM9DS0.h" -#include "../AP_HAL_Linux/GPIO.h" - -extern const AP_HAL::HAL& hal; - -//////////////////////////// -// LSM9DS0 Gyro Registers // -//////////////////////////// -#define WHO_AM_I_G 0x0F -#define CTRL_REG1_G 0x20 -#define CTRL_REG2_G 0x21 -#define CTRL_REG3_G 0x22 -#define CTRL_REG4_G 0x23 -#define CTRL_REG5_G 0x24 -#define REFERENCE_G 0x25 -#define STATUS_REG_G 0x27 -#define OUT_X_L_G 0x28 -#define OUT_X_H_G 0x29 -#define OUT_Y_L_G 0x2A -#define OUT_Y_H_G 0x2B -#define OUT_Z_L_G 0x2C -#define OUT_Z_H_G 0x2D -#define FIFO_CTRL_REG_G 0x2E -#define FIFO_SRC_REG_G 0x2F -#define INT1_CFG_G 0x30 -#define INT1_SRC_G 0x31 -#define INT1_THS_XH_G 0x32 -#define INT1_THS_XL_G 0x33 -#define INT1_THS_YH_G 0x34 -#define INT1_THS_YL_G 0x35 -#define INT1_THS_ZH_G 0x36 -#define INT1_THS_ZL_G 0x37 -#define INT1_DURATION_G 0x38 - -////////////////////////////////////////// -// LSM9DS0 Accel/Magneto (XM) Registers // -////////////////////////////////////////// -#define OUT_TEMP_L_XM 0x05 -#define OUT_TEMP_H_XM 0x06 -#define STATUS_REG_M 0x07 -#define OUT_X_L_M 0x08 -#define OUT_X_H_M 0x09 -#define OUT_Y_L_M 0x0A -#define OUT_Y_H_M 0x0B -#define OUT_Z_L_M 0x0C -#define OUT_Z_H_M 0x0D -#define WHO_AM_I_XM 0x0F -#define INT_CTRL_REG_M 0x12 -#define INT_SRC_REG_M 0x13 -#define INT_THS_L_M 0x14 -#define INT_THS_H_M 0x15 -#define OFFSET_X_L_M 0x16 -#define OFFSET_X_H_M 0x17 -#define OFFSET_Y_L_M 0x18 -#define OFFSET_Y_H_M 0x19 -#define OFFSET_Z_L_M 0x1A -#define OFFSET_Z_H_M 0x1B -#define REFERENCE_X 0x1C -#define REFERENCE_Y 0x1D -#define REFERENCE_Z 0x1E -#define CTRL_REG0_XM 0x1F -#define CTRL_REG1_XM 0x20 -#define CTRL_REG2_XM 0x21 -#define CTRL_REG3_XM 0x22 -#define CTRL_REG4_XM 0x23 -#define CTRL_REG5_XM 0x24 -#define CTRL_REG6_XM 0x25 -#define CTRL_REG7_XM 0x26 -#define STATUS_REG_A 0x27 -#define OUT_X_L_A 0x28 -#define OUT_X_H_A 0x29 -#define OUT_Y_L_A 0x2A -#define OUT_Y_H_A 0x2B -#define OUT_Z_L_A 0x2C -#define OUT_Z_H_A 0x2D -#define FIFO_CTRL_REG 0x2E -#define FIFO_SRC_REG 0x2F -#define INT_GEN_1_REG 0x30 -#define INT_GEN_1_SRC 0x31 -#define INT_GEN_1_THS 0x32 -#define INT_GEN_1_DURATION 0x33 -#define INT_GEN_2_REG 0x34 -#define INT_GEN_2_SRC 0x35 -#define INT_GEN_2_THS 0x36 -#define INT_GEN_2_DURATION 0x37 -#define CLICK_CFG 0x38 -#define CLICK_SRC 0x39 -#define CLICK_THS 0x3A -#define TIME_LIMIT 0x3B -#define TIME_LATENCY 0x3C -#define TIME_WINDOW 0x3D -#define ACT_THS 0x3E -#define ACT_DUR 0x3F - - -AP_InertialSensor_LSM9DS0::AP_InertialSensor_LSM9DS0(): - AP_InertialSensor(), - _drdy_pin_a(NULL), - _drdy_pin_m(NULL), - _drdy_pin_g(NULL), - _initialised(false), - _lsm9ds0_product_id(AP_PRODUCT_ID_NONE) -{ -} - -uint16_t AP_InertialSensor_LSM9DS0::_init_sensor( Sample_rate sample_rate) -{ - if (_initialised) return _lsm9ds0_product_id; - _initialised = true; - - _spi = hal.spi->device(AP_HAL::SPIDevice_LSM9DS0_AM); - _spi_sem = _spi->get_semaphore(); - - _drdy_pin_a = hal.gpio->channel(BBB_P8_8); - _drdy_pin_m = hal.gpio->channel(BBB_P8_10); - _drdy_pin_g = hal.gpio->channel(BBB_P8_34); - - // For some reason configuring the pins as an inputs make the driver fail - // _drdy_pin_a->mode(GPIO_IN); - // _drdy_pin_m->mode(GPIO_IN); - // _drdy_pin_g->mode(GPIO_IN); - - hal.scheduler->suspend_timer_procs(); - - 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("LSM9DS0: Unable to get semaphore")); - } - if (_data_ready()) { - _spi_sem->give(); - break; - } else { - hal.console->println_P( - PSTR("LSM9DS0 startup failed: no data ready")); - } - _spi_sem->give(); - } - if (tries++ > 5) { - hal.scheduler->panic(PSTR("PANIC: failed to boot LSM9DS0 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_g(); - _read_data_transaction_xm(); - _spi_sem->give(); - } - - // start the timer process to read samples - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_LSM9DS0::_poll_data, void)); - -#if LSM9DS0_DEBUG - _dump_registers(); -#endif - return _lsm9ds0_product_id; -} - - -/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */ - -bool AP_InertialSensor_LSM9DS0::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; -} - -// TODO finish -bool AP_InertialSensor_LSM9DS0::update( void ) -{ - // wait for at least 1 sample - if (!wait_for_sample(1000)) { - return false; - } - - _previous_accel[0] = _accel[0]; - - // disable timer procs for mininum time - hal.scheduler->suspend_timer_procs(); - _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); - _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); - // _mag[0] = Vector3f(_mag_sum.x, _mag_sum.y, _mag_sum.z); - - // TODO divide num_samples - _num_samples_g = _sum_count_g; - _num_samples_xm = _sum_count_xm; - - _accel_sum.zero(); - _gyro_sum.zero(); - _sum_count_g = 0; - _sum_count_xm = 0; - hal.scheduler->resume_timer_procs(); - - _gyro[0].rotate(_board_orientation); - _gyro[0] *= _gRes / _num_samples_g; - _gyro[0] -= _gyro_offset[0]; - - _accel[0].rotate(_board_orientation); - _accel[0] *= _aRes / _num_samples_xm; - - Vector3f accel_scaling = _accel_scale[0].get(); - _accel[0].x *= accel_scaling.x; - _accel[0].y *= accel_scaling.y; - _accel[0].z *= accel_scaling.z; - _accel[0] -= _accel_offset[0]; - - // // Configure mag - // _mag[0] *= _mRes / _num_samples_xm; - - // if (_last_filter_hz != _mpu6000_filter) { - // if (_spi_sem->take(10)) { - // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - // _set_filter_register(_mpu6000_filter, 0); - // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - // _error_count = 0; - // _spi_sem->give(); - // } - // } - return true; -} - -// return the LSM9DS0 gyro drift rate in radian/s/s -// TODO to be reviewed. Not sure about this value -float AP_InertialSensor_LSM9DS0::get_gyro_drift_rate(void) -{ - // 0.5 degrees/second/minute - return ToRad(0.5f/60); -} - -// get_delta_time returns the time period in seconds overwhich the sensor data was collected -float AP_InertialSensor_LSM9DS0::get_delta_time() const -{ - // the sensor runs at 200Hz - return (1.0f/700) * _num_samples_g; -} - - -/*================ HARDWARE FUNCTIONS ==================== */ - -// TODO finish the method -bool AP_InertialSensor_LSM9DS0::_hardware_init(Sample_rate sample_rate) -{ - - // Store the resolutions in private variables - _calcgRes(G_SCALE_245DPS); // Calculate DPS / ADC tick, stored in gRes variable - _calcmRes(M_SCALE_2GS); // Calculate Gs / ADC tick, stored in mRes variable - _calcaRes(A_SCALE_2G); // Calculate g / ADC tick, stored in aRes variable - - if (!_spi_sem->take(100)) { - hal.scheduler->panic(PSTR("LSM9DS0: Unable to get semaphore")); - } - - // initially run the bus at low speed (500kHz on APM2) - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); - - // Init the sensors - _initGyro(); - _initAccel(); - _initMag(); - - 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(_mpu6000_filter, default_filter); - - - // To verify communication, we can read from the WHO_AM_I register of - // each device. - uint8_t gTest = _register_read_g(WHO_AM_I_G); // Read the gyro WHO_AM_I - uint8_t xmTest = _register_read_xm(WHO_AM_I_XM); // Read the accel/mag WHO_AM_I - // TODO check the content of these variables. - - // now that we have initialised, we set the SPI bus speed to high - // (8MHz on APM2) - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); - - _spi_sem->give(); - - return true; -} - -/** - * Return true if the LSM9DS0 has new data available for reading. - * - * We use the data ready pin if it is available. - * Returns 1 if LSM9DS0 gyro is available, 2 if either the mag or the accel is available and - * 3 if both are. - * - * TODO, read the - * status register. - */ -uint8_t AP_InertialSensor_LSM9DS0::_data_ready() -{ - uint8_t rvalue = 0; - if (_drdy_pin_g) { - if (_drdy_pin_g->read() != 0){ - rvalue=1; - } - if (_drdy_pin_a) { - if (_drdy_pin_a->read() != 0){ - rvalue = 3; - } - } - } else if (_drdy_pin_a) { - if (_drdy_pin_a->read() != 0){ - rvalue = 2; - } - } - return rvalue; - - // TODO Implement a read on the status register - // uint8_t status = _register_read(MPUREG_INT_STATUS); - // return (status & BIT_RAW_RDY_INT) != 0; -} - - -/** - * Timer process to poll for new data from the LSM9DS0. - */ -void AP_InertialSensor_LSM9DS0::_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() == 1) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction_g(); - } else if (_data_ready() == 2){ - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction_xm(); - } else if (_data_ready() == 3){ - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction_g(); - _read_data_transaction_xm(); - } - _spi_sem->give(); - } else { - /* Synchronous read - take semaphore */ - if (_spi_sem->take(10)) { - if (_data_ready() == 1) { - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction_g(); - } else if (_data_ready() == 2){ - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction_xm(); - } else if (_data_ready() == 3){ - _last_sample_time_micros = hal.scheduler->micros(); - _read_data_transaction_g(); - _read_data_transaction_xm(); - } - _spi_sem->give(); - } else { - hal.scheduler->panic( - PSTR("PANIC: AP_InertialSensor_LSM9DS0::_poll_data " - "failed to take SPI semaphore synchronously")); - } - } -} - -// TODO use error_count to notifify if a transaction has gone wrong -void AP_InertialSensor_LSM9DS0::_read_data_transaction_g() { - - // read gyro values - uint8_t temp[6]; // We'll read six bytes from the gyro into temp - for (uint8_t i=0;i<6;i++){ - temp[i] = _register_read_g(OUT_X_L_G + i); - } - - uint16_t gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx - uint16_t gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy - uint16_t gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz - - _gyro_sum.x += gx; - _gyro_sum.y += gy; - _gyro_sum.z -= gz; - _sum_count_g++; - - if (_sum_count_g == 0) { - _gyro_sum.zero(); - } -} - -// TODO use error_count to notifify if a transaction has gone wrong -void AP_InertialSensor_LSM9DS0::_read_data_transaction_xm() { - - uint8_t temp[6]; // We'll read six bytes from the accel into temp - - // read accel values - for (uint8_t i=0;i<6;i++){ - temp[i] = _register_read_xm(OUT_X_L_A + i); - } - uint16_t ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax - uint16_t ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay - uint16_t az = (temp[5] << 8) | temp[4]; // Store z-axis values into az - - _accel_sum.x += ax; - _accel_sum.y += ay; - _accel_sum.z -= az; - - // read mag values - for (uint8_t i=0;i<6;i++){ - temp[i] = _register_read_xm(OUT_X_L_M + i); - } - uint16_t mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx - uint16_t my = (temp[3] << 8) | temp[2]; // Store y-axis values into my - uint16_t mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz - - _mag_sum.x += mx; - _mag_sum.y += my; - _mag_sum.z -= mz; - _sum_count_xm++; - - if (_sum_count_xm == 0) { - _gyro_sum.zero(); - _mag_sum.zero(); - } -} - -/*================ PRIVATE INTERFACE ==================== */ - - -// void AP_InertialSensor_LSM9DS0::_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); -// } -// } - - -// return true if a sample is available -bool AP_InertialSensor_LSM9DS0::_sample_available() -{ - _poll_data(); - return (_sum_count_g >> _sample_shift || _sum_count_xm >> _sample_shift) > 0; -} - -uint8_t AP_InertialSensor_LSM9DS0::_register_read_xm( 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]; -} - -uint8_t AP_InertialSensor_LSM9DS0::_register_read_g( 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_LSM9DS0::_register_write_xm(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); -} - -void AP_InertialSensor_LSM9DS0::_register_write_g(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); -} - -void AP_InertialSensor_LSM9DS0::_initGyro() -{ - /* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables - Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen - DR[1:0] - Output data rate selection - 00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz - BW[1:0] - Bandwidth selection (sets cutoff frequency) - Value depends on ODR. See datasheet table 21. - PD - Power down enable (0=power down mode, 1=normal or sleep mode) - Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */ - _register_write_g(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes - hal.scheduler->delay(1); - - /* CTRL_REG2_G sets up the HPF - Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0 - HPM[1:0] - High pass filter mode selection - 00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering, - 10=normal, 11=autoreset on interrupt - HPCF[3:0] - High pass filter cutoff frequency - Value depends on data rate. See datasheet table 26. - */ - _register_write_g(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency - hal.scheduler->delay(1); - - /* CTRL_REG3_G sets up interrupt and DRDY_G pins - Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY - I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable) - I1_BOOT - Boot status available on INT_G (0=disable, 1=enable) - H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low) - PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain) - I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable) - I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable) - I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable) - I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */ - // Int1 enabled (pp, active low), data read on DRDY_G: - _register_write_g(CTRL_REG3_G, 0x88); - hal.scheduler->delay(1); - - /* CTRL_REG4_G sets the scale, update mode - Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM - BDU - Block data update (0=continuous, 1=output not updated until read - BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add) - FS[1:0] - Full-scale selection - 00=245dps, 01=500dps, 10=2000dps, 11=2000dps - ST[1:0] - Self-test enable - 00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+) - SIM - SPI serial interface mode select - 0=4 wire, 1=3 wire */ - _register_write_g(CTRL_REG4_G, 0x00); // Set scale to 245 dps - hal.scheduler->delay(1); - - /* CTRL_REG5_G sets up the FIFO, HPF, and INT1 - Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0 - BOOT - Reboot memory content (0=normal, 1=reboot) - FIFO_EN - FIFO enable (0=disable, 1=enable) - HPen - HPF enable (0=disable, 1=enable) - INT1_Sel[1:0] - Int 1 selection configuration - Out_Sel[1:0] - Out selection configuration */ - _register_write_g(CTRL_REG5_G, 0x00); - hal.scheduler->delay(1); -} - -void AP_InertialSensor_LSM9DS0::_initAccel() -{ - /* CTRL_REG0_XM (0x1F) (Default value: 0x00) - Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2 - BOOT - Reboot memory content (0: normal, 1: reboot) - FIFO_EN - Fifo enable (0: disable, 1: enable) - WTM_EN - FIFO watermark enable (0: disable, 1: enable) - HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled) - HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled) - HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled) */ - _register_write_xm(CTRL_REG0_XM, 0x00); - hal.scheduler->delay(1); - - /* CTRL_REG1_XM (0x20) (Default value: 0x07) - Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN - AODR[3:0] - select the acceleration data rate: - 0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, - 0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz, - 1001=800Hz, 1010=1600Hz, (remaining combinations undefined). - BDU - block data update for accel AND mag - 0: Continuous update - 1: Output registers aren't updated until MSB and LSB have been read. - AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled. - 0: Axis disabled, 1: Axis enabled */ - _register_write_xm(CTRL_REG1_XM, 0x57); // 100Hz data rate, x/y/z all enabled - hal.scheduler->delay(1); - - //Serial.println(xmReadByte(CTRL_REG1_XM)); - /* CTRL_REG2_XM (0x21) (Default value: 0x00) - Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM - ABW[1:0] - Accelerometer anti-alias filter bandwidth - 00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz - AFS[2:0] - Accel full-scale selection - 000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g - AST[1:0] - Accel self-test enable - 00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed - SIM - SPI mode selection - 0=4-wire, 1=3-wire */ - _register_write_xm(CTRL_REG2_XM, 0x00); // Set scale to 2g - hal.scheduler->delay(1); - - /* CTRL_REG3_XM is used to set interrupt generators on INT1_XM - Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY - */ - // Accelerometer data ready on INT1_XM (0x04) - _register_write_xm(CTRL_REG3_XM, 0x04); - hal.scheduler->delay(1); -} - -void AP_InertialSensor_LSM9DS0::_initMag() -{ - /* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate - Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1 - TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled) - M_RES[1:0] - Magnetometer resolution select (0=low, 3=high) - M_ODR[2:0] - Magnetometer data rate select - 000=3.125Hz, 001=6.25Hz, 010=12.5Hz, 011=25Hz, 100=50Hz, 101=100Hz - LIR2 - Latch interrupt request on INT2_SRC (cleared by reading INT2_SRC) - 0=interrupt request not latched, 1=interrupt request latched - LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC) - 0=irq not latched, 1=irq latched */ - _register_write_xm(CTRL_REG5_XM, 0x14); // Mag data rate - 100 Hz - hal.scheduler->delay(1); - - /* CTRL_REG6_XM sets the magnetometer full-scale - Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0 - MFS[1:0] - Magnetic full-scale selection - 00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs */ - _register_write_xm(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS - hal.scheduler->delay(1); - - /* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters - AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0 - AHPM[1:0] - HPF mode selection - 00=normal (resets reference registers), 01=reference signal for filtering, - 10=normal, 11=autoreset on interrupt event - AFDS - Filtered acceleration data selection - 0=internal filter bypassed, 1=data from internal filter sent to FIFO - MLP - Magnetic data low-power mode - 0=data rate is set by M_ODR bits in CTRL_REG5 - 1=data rate is set to 3.125Hz - MD[1:0] - Magnetic sensor mode selection (default 10) - 00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */ - _register_write_xm(CTRL_REG7_XM, 0x00); // Continuous conversion mode - hal.scheduler->delay(1); - - /* CTRL_REG4_XM is used to set interrupt generators on INT2_XM - Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM - */ - _register_write_xm(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08) - hal.scheduler->delay(1); - - /* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high - Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN - XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data - PP_OD - Push-pull/open-drain interrupt configuration (0=push-pull, 1=od) - IEA - Interrupt polarity for accel and magneto - 0=active-low, 1=active-high - IEL - Latch interrupt request for accel and magneto - 0=irq not latched, 1=irq latched - 4D - 4D enable. 4D detection is enabled when 6D bit in INT_GEN1_REG is set - MIEN - Enable interrupt generation for magnetic data - 0=disable, 1=enable) */ - _register_write_xm(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull - hal.scheduler->delay(1); -} - - -void AP_InertialSensor_LSM9DS0::_calcgRes(gyro_scale_lsm9ds0 gScl) -{ - // Possible gyro scales (and their register bit settings) are: - // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm - // to calculate DPS/(ADC tick) based on that 2-bit value: - switch (gScl) - { - case G_SCALE_245DPS: - _gRes = 245.0f / 32768.0f; - break; - case G_SCALE_500DPS: - _gRes = 500.0f / 32768.0f; - break; - case G_SCALE_2000DPS: - _gRes = 2000.0f / 32768.0f; - break; - } -} - -void AP_InertialSensor_LSM9DS0::_calcaRes(accel_scale aScl) -{ - // Possible accelerometer scales (and their register bit settings) are: - // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an - // algorithm to calculate g/(ADC tick) based on that 3-bit value: - _aRes = aScl == A_SCALE_16G ? 16.0f / 32768.0f : - (((float) aScl + 1.0f) * 2.0f) / 32768.0f; -} - -void AP_InertialSensor_LSM9DS0::_calcmRes(mag_scale mScl) -{ - // Possible magnetometer scales (and their register bit settings) are: - // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm - // to calculate Gs/(ADC tick) based on that 2-bit value: - _mRes = mScl == M_SCALE_2GS ? 2.0f / 32768.0f : - (float) (mScl << 2) / 32768.0f; -} - -// TODO check the registers, dump first the Gyro registers and then the Mag registers -#if LSM9DS0_DEBUG -// dump all config registers - used for debug -void AP_InertialSensor_LSM9DS0::_dump_registers(void) -{ - hal.console->println_P(PSTR("LSM9DS0 registers:")); - hal.console->println_P(PSTR("Gyroscope registers:")); - const uint8_t first = OUT_TEMP_L_XM; - const uint8_t last = ACT_DUR; - for (uint8_t reg=first; reg<=last; reg++) { - uint8_t v = _register_read_g(reg); - hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); - if ((reg - (first-1)) % 16 == 0) { - hal.console->println(); - } - } - hal.console->println(); - - hal.console->println_P(PSTR("Accelerometer and Magnetometers registers:")); - for (uint8_t reg=first; reg<=last; reg++) { - uint8_t v = _register_read_xm(reg); - hal.console->printf_P(PSTR("%02x:%02x "), (unsigned)reg, (unsigned)v); - if ((reg - (first-1)) % 16 == 0) { - hal.console->println(); - } - } - hal.console->println(); - -} -#endif - -#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.h b/libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.h deleted file mode 100644 index 5bcca3b8c3..0000000000 --- a/libraries/AP_InertialSensor/LSM9DS0/AP_InertialSensor_LSM9DS0.h +++ /dev/null @@ -1,210 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __AP_INERTIAL_SENSOR_LSM9DS0_H__ -#define __AP_INERTIAL_SENSOR_LSM9DS0_H__ - -#include -#include -#include -#include -#include "AP_InertialSensor.h" - -// enable debug to see a register dump on startup -#define LSM9DS0_DEBUG 1 - -// gyro_scale_lsm9ds0 defines the possible full-scale ranges of the gyroscope: -enum gyro_scale_lsm9ds0 -{ - G_SCALE_245DPS, // 00: 245 degrees per second - G_SCALE_500DPS, // 01: 500 dps - G_SCALE_2000DPS, // 10: 2000 dps -}; -// accel_scale defines all possible FSR's of the accelerometer: -enum accel_scale -{ - A_SCALE_2G, // 000: 2g - A_SCALE_4G, // 001: 4g - A_SCALE_6G, // 010: 6g - A_SCALE_8G, // 011: 8g - A_SCALE_16G // 100: 16g -}; -// mag_scale defines all possible FSR's of the magnetometer: -enum mag_scale -{ - M_SCALE_2GS, // 00: 2Gs - M_SCALE_4GS, // 01: 4Gs - M_SCALE_8GS, // 10: 8Gs - M_SCALE_12GS, // 11: 12Gs -}; -// gyro_odr defines all possible data rate/bandwidth combos of the gyro: -enum gyro_odr -{ // ODR (Hz) --- Cutoff - G_ODR_95_BW_125 = 0x0, // 95 12.5 - G_ODR_95_BW_25 = 0x1, // 95 25 - // 0x2 and 0x3 define the same data rate and bandwidth - G_ODR_190_BW_125 = 0x4, // 190 12.5 - G_ODR_190_BW_25 = 0x5, // 190 25 - G_ODR_190_BW_50 = 0x6, // 190 50 - G_ODR_190_BW_70 = 0x7, // 190 70 - G_ODR_380_BW_20 = 0x8, // 380 20 - G_ODR_380_BW_25 = 0x9, // 380 25 - G_ODR_380_BW_50 = 0xA, // 380 50 - G_ODR_380_BW_100 = 0xB, // 380 100 - G_ODR_760_BW_30 = 0xC, // 760 30 - G_ODR_760_BW_35 = 0xD, // 760 35 - G_ODR_760_BW_50 = 0xE, // 760 50 - G_ODR_760_BW_100 = 0xF, // 760 100 -}; -// accel_oder defines all possible output data rates of the accelerometer: -enum accel_odr -{ - A_POWER_DOWN, // Power-down mode (0x0) - A_ODR_3125, // 3.125 Hz (0x1) - A_ODR_625, // 6.25 Hz (0x2) - A_ODR_125, // 12.5 Hz (0x3) - A_ODR_25, // 25 Hz (0x4) - A_ODR_50, // 50 Hz (0x5) - A_ODR_100, // 100 Hz (0x6) - A_ODR_200, // 200 Hz (0x7) - A_ODR_400, // 400 Hz (0x8) - A_ODR_800, // 800 Hz (9) - A_ODR_1600 // 1600 Hz (0xA) -}; - - // accel_abw defines all possible anti-aliasing filter rates of the accelerometer: -enum accel_abw -{ - A_ABW_773, // 773 Hz (0x0) - A_ABW_194, // 194 Hz (0x1) - A_ABW_362, // 362 Hz (0x2) - A_ABW_50, // 50 Hz (0x3) -}; - - -// mag_oder defines all possible output data rates of the magnetometer: -enum mag_odr -{ - M_ODR_3125, // 3.125 Hz (0x00) - M_ODR_625, // 6.25 Hz (0x01) - M_ODR_125, // 12.5 Hz (0x02) - M_ODR_25, // 25 Hz (0x03) - M_ODR_50, // 50 (0x04) - M_ODR_100, // 100 Hz (0x05) -}; - -class AP_InertialSensor_LSM9DS0: public AP_InertialSensor -{ -public: - AP_InertialSensor_LSM9DS0(); - - /* Concrete implementation of AP_InertialSensor functions: */ - bool update(); - float get_gyro_drift_rate(); - - // 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_gyro_health(uint8_t instance) const { return healthy(); } - 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_a; - AP_HAL::DigitalSource *_drdy_pin_m; - AP_HAL::DigitalSource *_drdy_pin_g; - uint32_t _gRes, _aRes, _mRes; - - void _calcgRes(gyro_scale_lsm9ds0 gScl); - void _calcaRes(accel_scale aScl); - void _calcmRes(mag_scale mScl); - - // initGyro() -- Sets up the gyroscope to begin reading. - // This function steps through all five gyroscope control registers. - // Upon exit, the following parameters will be set: - // - CTRL_REG1_G = 0x0F: Normal operation mode, all axes enabled. - // 95 Hz ODR, 12.5 Hz cutoff frequency. - // - CTRL_REG2_G = 0x00: HPF set to normal mode, cutoff frequency - // set to 7.2 Hz (depends on ODR). - // - CTRL_REG3_G = 0x88: Interrupt enabled on INT_G (set to push-pull and - // active high). Data-ready output enabled on DRDY_G. - // - CTRL_REG4_G = 0x00: Continuous update mode. Data LSB stored in lower - // address. Scale set to 245 DPS. SPI mode set to 4-wire. - // - CTRL_REG5_G = 0x00: FIFO disabled. HPF disabled. - void _initGyro(); - - // initAccel() -- Sets up the accelerometer to begin reading. - // This function steps through all accelerometer related control registers. - // Upon exit these registers will be set as: - // - CTRL_REG0_XM = 0x00: FIFO disabled. HPF bypassed. Normal mode. - // - CTRL_REG1_XM = 0x57: 100 Hz data rate. Continuous update. - // all axes enabled. - // - CTRL_REG2_XM = 0x00: 2g scale. 773 Hz anti-alias filter BW. - // - CTRL_REG3_XM = 0x04: Accel data ready signal on INT1_XM pin. - void _initAccel(); - - // initMag() -- Sets up the magnetometer to begin reading. - // This function steps through all magnetometer-related control registers. - // Upon exit these registers will be set as: - // - CTRL_REG4_XM = 0x04: Mag data ready signal on INT2_XM pin. - // - CTRL_REG5_XM = 0x14: 100 Hz update rate. Low resolution. Interrupt - // requests don't latch. Temperature sensor disabled. - // - CTRL_REG6_XM = 0x00: 2 Gs scale. - // - CTRL_REG7_XM = 0x00: Continuous conversion mode. Normal HPF mode. - // - INT_CTRL_REG_M = 0x09: Interrupt active-high. Enable interrupts. - void _initMag(); - - bool _sample_available(); - void _read_data_transaction_g(); - void _read_data_transaction_xm(); - uint8_t _data_ready(); - void _poll_data(void); - uint8_t _register_read_xm( uint8_t reg ); - uint8_t _register_read_g( uint8_t reg ); - void _register_write_xm( uint8_t reg, uint8_t val ); - void _register_write_g( uint8_t reg, uint8_t val ); - bool _hardware_init(Sample_rate sample_rate); - - AP_HAL::SPIDeviceDriver *_spi; - AP_HAL::Semaphore *_spi_sem; - - uint16_t _num_samples_g; - uint16_t _num_samples_xm; - static const float _gyro_scale; - - uint32_t _last_sample_time_micros; - - // ensure we can't initialise twice - bool _initialised; - int16_t _lsm9ds0_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 _gyro_sum; - Vector3l _mag_sum; - volatile int16_t _sum_count_g; - volatile int16_t _sum_count_xm; -#if LSM9DS0_DEBUG - void _dump_registers(void); -#endif -}; - - -#endif // __AP_INERTIAL_SENSOR_LSM9DS0_H__