From b058f3fdce9b5f14bd53d70fcb545af172bf6a78 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 8 Jan 2015 14:16:28 +1100
Subject: [PATCH] AP_InertialSensor: added missing files
---
.../AP_InertialSensor_Backend.cpp | 72 ++
.../AP_InertialSensor_Backend.h | 87 ++
.../AP_InertialSensor_L3GD20.cpp | 635 +++++++++++++
.../AP_InertialSensor_L3GD20.h | 88 ++
.../AP_InertialSensor_LSM303D.cpp | 831 ++++++++++++++++++
.../AP_InertialSensor_LSM303D.h | 107 +++
6 files changed, 1820 insertions(+)
create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp
create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h
create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp
create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
new file mode 100644
index 000000000..3ed5dd28f
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -0,0 +1,72 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#include <AP_HAL.h>
+#include "AP_InertialSensor.h"
+#include "AP_InertialSensor_Backend.h"
+
+AP_InertialSensor_Backend::AP_InertialSensor_Backend(AP_InertialSensor &imu) :
+ _imu(imu),
+ _product_id(AP_PRODUCT_ID_NONE)
+{}
+
+/*
+ rotate gyro vector and add the gyro offset
+ */
+void AP_InertialSensor_Backend::_rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro)
+{
+ _imu._gyro[instance] = gyro;
+ _imu._gyro[instance].rotate(_imu._board_orientation);
+ _imu._gyro[instance] -= _imu._gyro_offset[instance];
+ _imu._gyro_healthy[instance] = true;
+}
+
+/*
+ rotate accel vector, scale and add the accel offset
+ */
+void AP_InertialSensor_Backend::_rotate_and_offset_accel(uint8_t instance, const Vector3f &accel)
+{
+ _imu._accel[instance] = accel;
+ _imu._accel[instance].rotate(_imu._board_orientation);
+
+ const Vector3f &accel_scale = _imu._accel_scale[instance].get();
+ _imu._accel[instance].x *= accel_scale.x;
+ _imu._accel[instance].y *= accel_scale.y;
+ _imu._accel[instance].z *= accel_scale.z;
+ _imu._accel[instance] -= _imu._accel_offset[instance];
+ _imu._accel_healthy[instance] = true;
+}
+
+// set accelerometer error_count
+void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
+{
+ _imu._accel_error_count[instance] = error_count;
+}
+
+// set gyro error_count
+void AP_InertialSensor_Backend::_set_gyro_error_count(uint8_t instance, uint32_t error_count)
+{
+ _imu._gyro_error_count[instance] = error_count;
+}
+
+/*
+ return the default filter frequency in Hz for the sample rate
+
+ This uses the sample_rate as a proxy for what type of vehicle it is
+ (ie. plane and rover run at 50Hz). Copters need a bit more filter
+ bandwidth
+ */
+uint8_t AP_InertialSensor_Backend::_default_filter(void) const
+{
+ switch (_imu.get_sample_rate()) {
+ case AP_InertialSensor::RATE_50HZ:
+ // on Rover and plane use a lower filter rate
+ return 15;
+ case AP_InertialSensor::RATE_100HZ:
+ return 30;
+ case AP_InertialSensor::RATE_200HZ:
+ return 30;
+ case AP_InertialSensor::RATE_400HZ:
+ return 30;
+ }
+ return 30;
+}
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
new file mode 100644
index 000000000..acd6254ba
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -0,0 +1,87 @@
+// -*- 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 <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ IMU driver backend class. Each supported gyro/accel sensor type
+ needs to have an object derived from this class.
+
+ Note that drivers can implement just gyros or just accels, and can
+ also provide multiple gyro/accel instances.
+ */
+#ifndef __AP_INERTIALSENSOR_BACKEND_H__
+#define __AP_INERTIALSENSOR_BACKEND_H__
+
+class AP_InertialSensor_Backend
+{
+public:
+ AP_InertialSensor_Backend(AP_InertialSensor &imu);
+
+ // we declare a virtual destructor so that drivers can
+ // override with a custom destructor if need be.
+ virtual ~AP_InertialSensor_Backend(void) {}
+
+ /*
+ * Update the sensor data. Called by the frontend to transfer
+ * accumulated sensor readings to the frontend structure via the
+ * _rotate_and_offset_gyro() and _rotate_and_offset_accel() functions
+ */
+ virtual bool update() = 0;
+
+ /*
+ * return true if at least one accel sample is available in the backend
+ * since the last call to update()
+ */
+ virtual bool accel_sample_available() = 0;
+
+ /*
+ * return true if at least one gyro sample is available in the backend
+ * since the last call to update()
+ */
+ virtual bool gyro_sample_available() = 0;
+
+ /*
+ return the product ID
+ */
+ int16_t product_id(void) const { return _product_id; }
+
+protected:
+ // access to frontend
+ AP_InertialSensor &_imu;
+
+ // rotate gyro vector and offset
+ void _rotate_and_offset_gyro(uint8_t instance, const Vector3f &gyro);
+
+ // rotate accel vector, scale and offset
+ void _rotate_and_offset_accel(uint8_t instance, const Vector3f &accel);
+
+ // set accelerometer error_count
+ void _set_accel_error_count(uint8_t instance, uint32_t error_count);
+
+ // set gyro error_count
+ void _set_gyro_error_count(uint8_t instance, uint32_t error_count);
+
+ // backend should fill in its product ID from AP_PRODUCT_ID_*
+ int16_t _product_id;
+
+ // return the default filter frequency in Hz for the sample rate
+ uint8_t _default_filter(void) const;
+
+ // note that each backend is also expected to have a static detect()
+ // function which instantiates an instance of the backend sensor
+ // driver if the sensor is available
+};
+
+#endif // __AP_INERTIALSENSOR_BACKEND_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp
new file mode 100644
index 000000000..6a443398e
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.cpp
@@ -0,0 +1,635 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ * Coded by VÃctor Mayoral Vilches <v.mayoralv@gmail.com> using
+ * l3gd20.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>
+#if NOT_YET
+
+#include "AP_InertialSensor_L3GD20.h"
+
+extern const AP_HAL::HAL& hal;
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ #define L3GD20_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 L3GD20_DRDY_PIN BBB_P8_34 // GYRO_DRDY
+ #endif
+#endif
+
+/* L3GD20 definitions */
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG 0
+#define SENSOR_BOARD_ROTATION_090_DEG 1
+#define SENSOR_BOARD_ROTATION_180_DEG 2
+#define SENSOR_BOARD_ROTATION_270_DEG 3
+
+/* SPI protocol address bits */
+#define DIR_READ (1<<7)
+#define DIR_WRITE (0<<7)
+#define ADDR_INCREMENT (1<<6)
+
+/* register addresses */
+#define ADDR_WHO_AM_I 0x0F
+#define WHO_I_AM_H 0xD7
+#define WHO_I_AM 0xD4
+
+#define ADDR_CTRL_REG1 0x20
+#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
+/* keep lowpass low to avoid noise issues */
+#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
+#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
+#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
+#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
+#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
+#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
+#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
+#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
+#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
+
+#define ADDR_CTRL_REG2 0x21
+#define ADDR_CTRL_REG3 0x22
+#define ADDR_CTRL_REG4 0x23
+#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */
+#define RANGE_250DPS (0<<4)
+#define RANGE_500DPS (1<<4)
+#define RANGE_2000DPS (3<<4)
+
+#define ADDR_CTRL_REG5 0x24
+#define ADDR_REFERENCE 0x25
+#define ADDR_OUT_TEMP 0x26
+#define ADDR_STATUS_REG 0x27
+#define ADDR_OUT_X_L 0x28
+#define ADDR_OUT_X_H 0x29
+#define ADDR_OUT_Y_L 0x2A
+#define ADDR_OUT_Y_H 0x2B
+#define ADDR_OUT_Z_L 0x2C
+#define ADDR_OUT_Z_H 0x2D
+#define ADDR_FIFO_CTRL_REG 0x2E
+#define ADDR_FIFO_SRC_REG 0x2F
+#define ADDR_INT1_CFG 0x30
+#define ADDR_INT1_SRC 0x31
+#define ADDR_INT1_TSH_XH 0x32
+#define ADDR_INT1_TSH_XL 0x33
+#define ADDR_INT1_TSH_YH 0x34
+#define ADDR_INT1_TSH_YL 0x35
+#define ADDR_INT1_TSH_ZH 0x36
+#define ADDR_INT1_TSH_ZL 0x37
+#define ADDR_INT1_DURATION 0x38
+
+/* Internal configuration values */
+#define REG1_POWER_NORMAL (1<<3)
+#define REG1_Z_ENABLE (1<<2)
+#define REG1_Y_ENABLE (1<<1)
+#define REG1_X_ENABLE (1<<0)
+
+#define REG4_BDU (1<<7)
+#define REG4_BLE (1<<6)
+//#define REG4_SPI_3WIRE (1<<0)
+
+#define REG5_FIFO_ENABLE (1<<6)
+#define REG5_REBOOT_MEMORY (1<<7)
+
+#define STATUS_ZYXOR (1<<7)
+#define STATUS_ZOR (1<<6)
+#define STATUS_YOR (1<<5)
+#define STATUS_XOR (1<<4)
+#define STATUS_ZYXDA (1<<3)
+#define STATUS_ZDA (1<<2)
+#define STATUS_YDA (1<<1)
+#define STATUS_XDA (1<<0)
+
+#define FIFO_CTRL_BYPASS_MODE (0<<5)
+#define FIFO_CTRL_FIFO_MODE (1<<5)
+#define FIFO_CTRL_STREAM_MODE (1<<6)
+#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
+#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
+
+#define L3GD20_DEFAULT_RATE 760
+#define L3GD20_DEFAULT_RANGE_DPS 2000
+#define L3GD20_DEFAULT_FILTER_FREQ 30
+
+
+// const float AP_InertialSensor_L3GD20::_gyro_scale = (0.0174532f / 16.4f);
+
+
+AP_InertialSensor_L3GD20::AP_InertialSensor_L3GD20() :
+ AP_InertialSensor(),
+ _drdy_pin(NULL),
+ _initialised(false),
+ _L3GD20_product_id(AP_PRODUCT_ID_NONE)
+{
+}
+
+uint16_t AP_InertialSensor_L3GD20::_init_sensor( Sample_rate sample_rate )
+{
+ if (_initialised) return _L3GD20_product_id;
+ _initialised = true;
+
+ _spi = hal.spi->device(AP_HAL::SPIDevice_L3GD20);
+ _spi_sem = _spi->get_semaphore();
+
+#ifdef L3GD20_DRDY_PIN
+ _drdy_pin = hal.gpio->channel(L3GD20_DRDY_PIN);
+ _drdy_pin->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("L3GD20: unexpected WHOAMI 0x%x\n", (unsigned)whoami);
+ hal.scheduler->panic(PSTR("L3GD20: 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("L3GD20: Unable to get semaphore"));
+ }
+ if (_data_ready()) {
+ _spi_sem->give();
+ break;
+ } else {
+ hal.console->println_P(
+ PSTR("L3GD20 startup failed: no data ready"));
+ }
+ _spi_sem->give();
+ }
+ if (tries++ > 5) {
+ hal.scheduler->panic(PSTR("PANIC: failed to boot L3GD20 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_L3GD20::_poll_data));
+
+#if L3GD20_DEBUG
+ _dump_registers();
+#endif
+ return _L3GD20_product_id;
+}
+
+/*================ AP_INERTIALSENSOR PUBLIC INTERFACE ==================== */
+
+bool AP_InertialSensor_L3GD20::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_L3GD20::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();
+ _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z);
+ _num_samples = _sum_count;
+ _gyro_sum.zero();
+ _sum_count = 0;
+ hal.scheduler->resume_timer_procs();
+
+ _gyro[0].rotate(_board_orientation);
+ _gyro[0] *= _gyro_scale / _num_samples;
+ _gyro[0] -= _gyro_offset[0];
+
+ // if (_last_filter_hz != _L3GD20_filter) {
+ // if (_spi_sem->take(10)) {
+ // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW);
+ // _set_filter_register(_L3GD20_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 L3GD20 has new data available for reading.
+ *
+ * We use the data ready pin if it is available. Otherwise, read the
+ * status register.
+ */
+bool AP_InertialSensor_L3GD20::_data_ready()
+{
+ if (_drdy_pin) {
+ return _drdy_pin->read() != 0;
+ }
+ // TODO: read status register
+ return false;
+}
+
+/**
+ * Timer process to poll for new data from the L3GD20.
+ */
+void AP_InertialSensor_L3GD20::_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_L3GD20::_poll_data "
+ "failed to take SPI semaphore synchronously"));
+ }
+ }
+}
+
+void AP_InertialSensor_L3GD20::_read_data_transaction() {
+
+ struct {
+ uint8_t cmd;
+ uint8_t temp;
+ uint8_t status;
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ } raw_report;
+
+ /* fetch data from the sensor */
+ memset(&raw_report, 0, sizeof(raw_report));
+ raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
+ _spi->transaction((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
+
+#if L3GD20_USE_DRDY
+ if ((raw_report.status & 0xF) != 0xF) {
+ /*
+ we waited for DRDY, but did not see DRDY on all axes
+ when we captured. That means a transfer error of some sort
+ */
+ hal.console->println_P(
+ PSTR("L3GD20: DRDY is not on in all axes, transfer error"));
+ return;
+ }
+#endif
+ _gyro_sum.x += raw_report.x;
+ _gyro_sum.y += raw_report.y;
+ _gyro_sum.z -= raw_report.z;
+ _sum_count++;
+
+ if (_sum_count == 0) {
+ // rollover - v unlikely
+ _gyro_sum.zero();
+ }
+}
+
+uint8_t AP_InertialSensor_L3GD20::_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_L3GD20::_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_L3GD20::_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 L3GD20_DEBUG
+ hal.console->printf_P(PSTR("Values written: %02x; readed: %02x "), val, readed);
+#endif
+}
+
+/*
+ set the DLPF filter frequency. Assumes caller has taken semaphore
+ TODO needs to be changed according to L3GD20 needs
+ */
+// void AP_InertialSensor_L3GD20::_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_L3GD20::disable_i2c(void)
+{
+ uint8_t retries = 10;
+ while (retries--) {
+ // add retries
+ uint8_t a = _register_read(0x05);
+ _register_write(0x05, (0x20 | a));
+ if (_register_read(0x05) == (a | 0x20)) {
+ return;
+ }
+ }
+ hal.scheduler->panic(PSTR("L3GD20: Unable to disable I2C"));
+}
+
+uint8_t AP_InertialSensor_L3GD20::set_samplerate(uint16_t frequency)
+{
+ uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
+ if (frequency == 0)
+ frequency = 760;
+
+ /* use limits good for H or non-H models */
+ if (frequency <= 100) {
+ // _current_rate = 95;
+ bits |= RATE_95HZ_LP_25HZ;
+
+ } else if (frequency <= 200) {
+ // _current_rate = 190;
+ bits |= RATE_190HZ_LP_50HZ;
+
+ } else if (frequency <= 400) {
+ // _current_rate = 380;
+ bits |= RATE_380HZ_LP_50HZ;
+
+ } else if (frequency <= 800) {
+ // _current_rate = 760;
+ bits |= RATE_760HZ_LP_50HZ;
+ } else {
+ return -1;
+ }
+ _register_write(ADDR_CTRL_REG1, bits);
+ return 0;
+}
+
+uint8_t AP_InertialSensor_L3GD20::set_range(uint8_t max_dps)
+{
+ uint8_t bits = REG4_BDU;
+ float new_range_scale_dps_digit;
+ float new_range;
+
+ if (max_dps == 0) {
+ max_dps = 2000;
+ }
+ if (max_dps <= 250) {
+ new_range = 250;
+ bits |= RANGE_250DPS;
+ new_range_scale_dps_digit = 8.75e-3f;
+
+ } else if (max_dps <= 500) {
+ new_range = 500;
+ bits |= RANGE_500DPS;
+ new_range_scale_dps_digit = 17.5e-3f;
+
+ } else if (max_dps <= 2000) {
+ new_range = 2000;
+ bits |= RANGE_2000DPS;
+ new_range_scale_dps_digit = 70e-3f;
+
+ } else {
+ return -1;
+ }
+
+ // _gyro_range_rad_s = new_range / 180.0f * M_PI_F;
+ // _gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
+ _gyro_scale = new_range_scale_dps_digit / 180.0f * M_PI_F;
+ _register_write(ADDR_CTRL_REG4, bits);
+ return 0;
+}
+
+bool AP_InertialSensor_L3GD20::_hardware_init(Sample_rate sample_rate)
+{
+ if (!_spi_sem->take(100)) {
+ hal.scheduler->panic(PSTR("L3GD20: 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();
+
+ // Chip reset
+ /* set default configuration */
+ _register_write(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+ hal.scheduler->delay(1);
+ _register_write(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
+ hal.scheduler->delay(1);
+ _register_write(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
+ hal.scheduler->delay(1);
+ _register_write(ADDR_CTRL_REG4, REG4_BDU);
+ hal.scheduler->delay(1);
+ _register_write(ADDR_CTRL_REG5, 0);
+ hal.scheduler->delay(1);
+
+ _register_write(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
+ hal.scheduler->delay(1);
+
+ /* disable FIFO. This makes things simpler and ensures we
+ * aren't getting stale data. It means we must run the hrt
+ * callback fast enough to not miss data. */
+ _register_write(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
+ hal.scheduler->delay(1);
+
+ set_samplerate(0); // 760Hz
+ hal.scheduler->delay(1);
+ set_range(L3GD20_DEFAULT_RANGE_DPS);
+ hal.scheduler->delay(1);
+
+ // //TODO: Filtering
+ // 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(_L3GD20_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 the MPU6k gyro drift rate in radian/s/s
+// note that this is much better than the oilpan gyros
+float AP_InertialSensor_L3GD20::get_gyro_drift_rate(void)
+{
+ // 0.5 degrees/second/minute
+ return ToRad(0.5/60);
+}
+
+// return true if a sample is available
+bool AP_InertialSensor_L3GD20::_sample_available()
+{
+ _poll_data();
+ // return (_sum_count >> _sample_shift) > 0;
+ return (_sum_count) > 0;
+}
+
+
+#if L3GD20_DEBUG
+// dump all config registers - used for debug
+void AP_InertialSensor_L3GD20::_dump_registers(void)
+{
+ hal.console->println_P(PSTR("L3GD20 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_L3GD20::get_delta_time() const
+{
+ // the sensor runs at 200Hz
+ return 0.005 * _num_samples;
+}
+#endif
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h
new file mode 100644
index 000000000..ca7c50233
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_L3GD20.h
@@ -0,0 +1,88 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_INERTIAL_SENSOR_L3GD20_H__
+#define __AP_INERTIAL_SENSOR_L3GD20_H__
+
+#include <stdint.h>
+#include <AP_HAL.h>
+#include <AP_Math.h>
+#include <AP_Progmem.h>
+#include "AP_InertialSensor.h"
+
+// enable debug to see a register dump on startup
+#define L3GD20_DEBUG 0
+
+class AP_InertialSensor_L3GD20 : public AP_InertialSensor
+{
+public:
+
+ AP_InertialSensor_L3GD20();
+
+ /* 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;
+
+ bool _sample_available();
+ void _read_data_transaction();
+ bool _data_ready();
+ void _poll_data(void);
+ uint8_t _register_read( uint8_t reg );
+ void _register_write( uint8_t reg, uint8_t val );
+ void _register_write_check(uint8_t reg, uint8_t val);
+ bool _hardware_init(Sample_rate sample_rate);
+ void disable_i2c(void);
+ uint8_t set_samplerate(uint16_t frequency);
+ uint8_t set_range(uint8_t max_dps);
+
+ AP_HAL::SPIDeviceDriver *_spi;
+ AP_HAL::Semaphore *_spi_sem;
+
+ uint16_t _num_samples;
+ float _gyro_scale;
+
+ uint32_t _last_sample_time_micros;
+
+ // ensure we can't initialise twice
+ bool _initialised;
+ int16_t _L3GD20_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 _gyro_sum;
+ volatile int16_t _sum_count;
+
+public:
+
+#if L3GD20_DEBUG
+ void _dump_registers(void);
+#endif
+};
+
+#endif // __AP_INERTIAL_SENSOR_L3GD20_H__
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp
new file mode 100644
index 000000000..b6c6b7d34
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.cpp
@@ -0,0 +1,831 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#if NOT_YET
+
+/****************************************************************************
+ *
+ * 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);
+ 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(uint16_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(uint16_t frequency)
+{
+ uint8_t setbits = 0;
+ uint8_t clearbits = REG5_RATE_BITS_M;
+
+ if (frequency == 0)
+ frequency = 100;
+
+ if (frequency <= 25) {
+ setbits |= REG5_RATE_25HZ_M;
+ _mag_samplerate = 25;
+
+ } else if (frequency <= 50) {
+ setbits |= REG5_RATE_50HZ_M;
+ _mag_samplerate = 50;
+
+ } else if (frequency <= 100) {
+ setbits |= REG5_RATE_100HZ_M;
+ _mag_samplerate = 100;
+
+ } else {
+ return -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;
+}
+#endif
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h
new file mode 100644
index 000000000..a2c6fa2d1
--- /dev/null
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_LSM303D.h
@@ -0,0 +1,107 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef __AP_INERTIAL_SENSOR_LSM303D_H__
+#define __AP_INERTIAL_SENSOR_LSM303D_H__
+
+#include <stdint.h>
+#include <AP_HAL.h>
+#include <AP_Math.h>
+#include <AP_Progmem.h>
+#include "AP_InertialSensor.h"
+
+// enable debug to see a register dump on startup
+#define LSM303D_DEBUG 0
+
+class AP_InertialSensor_LSM303D: public AP_InertialSensor
+{
+public:
+
+ AP_InertialSensor_LSM303D();
+
+ /* Concrete implementation of AP_InertialSensor functions: */
+ bool update();
+
+ // wait for a sample to be available, with timeout in milliseconds
+ bool wait_for_sample(uint16_t timeout_ms);
+
+ // get_delta_time returns the time period in seconds overwhich the sensor data was collected
+ float get_delta_time() const;
+
+ uint16_t error_count(void) const { return _error_count; }
+ bool healthy(void) const { return _error_count <= 4; }
+ bool get_accel_health(uint8_t instance) const { return healthy(); }
+
+protected:
+ uint16_t _init_sensor( Sample_rate sample_rate );
+
+private:
+ AP_HAL::DigitalSource *_drdy_pin_x;
+ AP_HAL::DigitalSource *_drdy_pin_m;
+ uint8_t _accel_range_m_s2;
+ float _accel_range_scale;
+ uint8_t _accel_samplerate;
+ uint8_t _accel_onchip_filter_bandwith;
+ uint8_t _mag_range_ga;
+ float _mag_range_scale;
+ uint8_t _mag_samplerate;
+
+ // expceted values of reg1 and reg7 to catch in-flight
+ // brownouts of the sensor
+ uint8_t _reg1_expected;
+ uint8_t _reg7_expected;
+
+ bool _sample_available();
+ void _read_data_transaction();
+ void _read_data_transaction_accel();
+ void _read_data_transaction_mag();
+ bool _data_ready();
+ void _poll_data(void);
+ uint8_t _register_read( uint8_t reg );
+ void _register_write( uint8_t reg, uint8_t val );
+ void _register_write_check(uint8_t reg, uint8_t val);
+ void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits);
+ bool _hardware_init(Sample_rate sample_rate);
+ void disable_i2c(void);
+ uint8_t accel_set_range(uint8_t max_g);
+ uint8_t accel_set_samplerate(uint16_t frequency);
+ uint8_t accel_set_onchip_lowpass_filter_bandwidth(uint8_t bandwidth);
+ uint8_t mag_set_range(uint8_t max_ga);
+ uint8_t mag_set_samplerate(uint16_t frequency);
+
+ AP_HAL::SPIDeviceDriver *_spi;
+ AP_HAL::Semaphore *_spi_sem;
+
+ uint16_t _num_samples;
+ float _accel_scale;
+ float _mag_scale;
+
+ uint32_t _last_sample_time_micros;
+
+ // ensure we can't initialise twice
+ bool _initialised;
+ int16_t _LSM303D_product_id;
+
+ // how many hardware samples before we report a sample to the caller
+ uint8_t _sample_shift;
+
+ // support for updating filter at runtime
+ uint8_t _last_filter_hz;
+
+ void _set_filter_register(uint8_t filter_hz, uint8_t default_filter);
+
+ uint16_t _error_count;
+
+ // accumulation in timer - must be read with timer disabled
+ // the sum of the values since last read
+ Vector3l _accel_sum;
+ Vector3l _mag_sum;
+ volatile int16_t _sum_count;
+
+public:
+
+#if LSM303D_DEBUG
+ void _dump_registers(void);
+#endif
+};
+
+#endif // __AP_INERTIAL_SENSOR_LSM303D_H__
--
GitLab