diff --git a/src/drivers/imu/lsm303d/CMakeLists.txt b/src/drivers/imu/lsm303d/CMakeLists.txt index e76ce8894a2c..38d49e2955bb 100644 --- a/src/drivers/imu/lsm303d/CMakeLists.txt +++ b/src/drivers/imu/lsm303d/CMakeLists.txt @@ -37,8 +37,10 @@ px4_add_module( COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable SRCS - lsm303d.cpp + lsm303d_main.cpp + LSM303D.cpp DEPENDS drivers_accelerometer drivers_magnetometer + px4_work_queue ) diff --git a/src/drivers/imu/lsm303d/lsm303d.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp similarity index 57% rename from src/drivers/imu/lsm303d/lsm303d.cpp rename to src/drivers/imu/lsm303d/LSM303D.cpp index fb1f3c026162..3ecf4a021bc9 100644 --- a/src/drivers/imu/lsm303d/lsm303d.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -32,315 +32,11 @@ ****************************************************************************/ /** - * @file lsm303d.cpp + * @file LSM303D.cpp * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. */ -#include -#include -#include -#include -#include -#include -#include - -/* 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 REG_STATUS_A_NEW_ZYXADA 0x08 - -#define INT_CTRL_M 0x12 -#define INT_SRC_M 0x13 - -/* default values for this device */ -#define LSM303D_ACCEL_DEFAULT_RANGE_G 16 -#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 - -/* - we set the timer interrupt to run a bit faster than the desired - sample rate and then throw away duplicates using the data ready bit. - This time reduction is enough to cope with worst case timing jitter - due to other timers - */ -#define LSM303D_TIMER_REDUCTION 200 - -extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } - -class LSM303D : public device::SPI, public px4::ScheduledWorkItem -{ -public: - LSM303D(int bus, uint32_t device, enum Rotation rotation); - virtual ~LSM303D(); - - virtual int init(); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - virtual int probe(); - -private: - - void Run() override; - - void start(); - void stop(); - void reset(); - - /** - * disable I2C on the chip - */ - void disable_i2c(); - - /** - * check key registers for correct values - */ - void check_registers(void); - - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - void measureAccelerometer(); - - /** - * Fetch mag measurements from the sensor and update the report ring. - */ - void measureMagnetometer(); - - /** - * Read a register from the LSM303D - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the LSM303D - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the LSM303D - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the LSM303D, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - - /** - * Set the LSM303D accel measurement range. - * - * @param max_g The measurement range of the accel is in g (9.81m/s^2) - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_range(unsigned max_g); - - /** - * Set the LSM303D mag measurement range. - * - * @param max_ga The measurement range of the mag is in Ga - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int mag_set_range(unsigned max_g); - - /** - * Set the LSM303D on-chip anti-alias filter bandwith. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * Zero selects the highest bandwidth - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); - - /** - * Set the LSM303D internal accel sampling frequency. - * - * @param frequency The internal accel sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int accel_set_samplerate(unsigned frequency); - - /** - * Set the LSM303D internal mag sampling frequency. - * - * @param frequency The internal mag sampling frequency is set to not less than - * this value. - * Zero selects the maximum rate supported. - * @return OK if the value can be supported. - */ - int mag_set_samplerate(unsigned frequency); - - - PX4Accelerometer _px4_accel; - PX4Magnetometer _px4_mag; - - unsigned _call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE}; - unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE}; - - perf_counter_t _accel_sample_perf; - perf_counter_t _accel_sample_interval_perf; - perf_counter_t _mag_sample_perf; - perf_counter_t _mag_sample_interval_perf; - perf_counter_t _bad_registers; - perf_counter_t _bad_values; - perf_counter_t _accel_duplicates; - - uint8_t _register_wait{0}; - - int16_t _last_accel[3] {}; - uint8_t _constant_accel_count{0}; - - hrt_abstime _mag_last_measure{0}; - - float _last_temperature{0.0f}; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset - static constexpr int LSM303D_NUM_CHECKED_REGISTERS{8}; - uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS] {}; - uint8_t _checked_next{0}; - -}; +#include "LSM303D.hpp" /* list of registers that will be checked in check_registers(). Note @@ -372,6 +68,7 @@ LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) : { _px4_accel.set_device_type(DRV_ACC_DEVTYPE_LSM303D); _px4_mag.set_device_type(DRV_MAG_DEVTYPE_LSM303D); + _px4_mag.set_external(external()); } LSM303D::~LSM303D() @@ -914,144 +611,3 @@ LSM303D::print_info() } } } - -/** - * Local functions in support of the shell command. - */ -namespace lsm303d -{ - -LSM303D *g_dev; - -void start(bool external_bus, enum Rotation rotation, unsigned range); -void info(); -void usage(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ -void -start(bool external_bus, enum Rotation rotation, unsigned range) -{ - if (g_dev != nullptr) { - errx(0, "already started"); - } - - /* create the driver */ - if (external_bus) { -#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) - g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); -#else - errx(0, "External SPI not available"); -#endif - - } else { - g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation); - } - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating LSM303D obj"); - goto fail; - } - - if (OK != g_dev->init()) { - goto fail; - } - - exit(0); -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - errx(1, "driver start failed"); -} - -/** - * Print a little info about the driver. - */ -void -info() -{ - if (g_dev == nullptr) { - errx(1, "driver not running\n"); - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - exit(0); -} - -void -usage() -{ - PX4_INFO("missing command: try 'start', 'info'"); - PX4_INFO("options:"); - PX4_INFO(" -X (external bus)"); - PX4_INFO(" -R rotation"); -} - -} // namespace - -int -lsm303d_main(int argc, char *argv[]) -{ - bool external_bus = false; - enum Rotation rotation = ROTATION_NONE; - int accel_range = 8; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'X': - external_bus = true; - break; - - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - case 'a': - accel_range = atoi(myoptarg); - break; - - default: - lsm303d::usage(); - exit(0); - } - } - - if (myoptind >= argc) { - lsm303d::usage(); - exit(0); - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - - */ - if (!strcmp(verb, "start")) { - lsm303d::start(external_bus, rotation, accel_range); - } - - /* - * Print driver information. - */ - if (!strcmp(verb, "info")) { - lsm303d::info(); - } - - errx(1, "unrecognized command, try 'start', info'"); -} diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp new file mode 100644 index 000000000000..ab9abf39d380 --- /dev/null +++ b/src/drivers/imu/lsm303d/LSM303D.hpp @@ -0,0 +1,300 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * 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. + * + ****************************************************************************/ + +/** + * @file LSM303D.hpp + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +/* 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_STATUS_A 0x27 + +#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 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 REG_STATUS_A_NEW_ZYXADA 0x08 + +/* default values for this device */ +#define LSM303D_ACCEL_DEFAULT_RANGE_G 16 +#define LSM303D_ACCEL_DEFAULT_RATE 800 +#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 + +#define LSM303D_MAG_DEFAULT_RANGE_GA 2 +#define LSM303D_MAG_DEFAULT_RATE 100 + +/* + we set the timer interrupt to run a bit faster than the desired + sample rate and then throw away duplicates using the data ready bit. + This time reduction is enough to cope with worst case timing jitter + due to other timers + */ +#define LSM303D_TIMER_REDUCTION 200 + +extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } + +class LSM303D : public device::SPI, public px4::ScheduledWorkItem +{ +public: + LSM303D(int bus, uint32_t device, enum Rotation rotation); + virtual ~LSM303D(); + + int init() override; + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + + void Run() override; + + void start(); + void stop(); + void reset(); + + /** + * disable I2C on the chip + */ + void disable_i2c(); + + /** + * check key registers for correct values + */ + void check_registers(void); + + /** + * Fetch accel measurements from the sensor and update the report ring. + */ + void measureAccelerometer(); + + /** + * Fetch mag measurements from the sensor and update the report ring. + */ + void measureMagnetometer(); + + /** + * Read a register from the LSM303D + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the LSM303D + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the LSM303D + * + * Bits are cleared before bits are set. + * + * @param reg The register to modify. + * @param clearbits Bits in the register to clear. + * @param setbits Bits in the register to set. + */ + void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Write a register in the LSM303D, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + + /** + * Set the LSM303D accel measurement range. + * + * @param max_g The measurement range of the accel is in g (9.81m/s^2) + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_range(unsigned max_g); + + /** + * Set the LSM303D mag measurement range. + * + * @param max_ga The measurement range of the mag is in Ga + * Zero selects the maximum supported range. + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int mag_set_range(unsigned max_g); + + /** + * Set the LSM303D on-chip anti-alias filter bandwith. + * + * @param bandwidth The anti-alias filter bandwidth in Hz + * Zero selects the highest bandwidth + * @return OK if the value can be supported, -ERANGE otherwise. + */ + int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth); + + /** + * Set the LSM303D internal accel sampling frequency. + * + * @param frequency The internal accel sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int accel_set_samplerate(unsigned frequency); + + /** + * Set the LSM303D internal mag sampling frequency. + * + * @param frequency The internal mag sampling frequency is set to not less than + * this value. + * Zero selects the maximum rate supported. + * @return OK if the value can be supported. + */ + int mag_set_samplerate(unsigned frequency); + + + PX4Accelerometer _px4_accel; + PX4Magnetometer _px4_mag; + + unsigned _call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE}; + unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE}; + + perf_counter_t _accel_sample_perf; + perf_counter_t _accel_sample_interval_perf; + perf_counter_t _mag_sample_perf; + perf_counter_t _mag_sample_interval_perf; + perf_counter_t _bad_registers; + perf_counter_t _bad_values; + perf_counter_t _accel_duplicates; + + uint8_t _register_wait{0}; + + int16_t _last_accel[3] {}; + uint8_t _constant_accel_count{0}; + + hrt_abstime _mag_last_measure{0}; + + float _last_temperature{0.0f}; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset + static constexpr int LSM303D_NUM_CHECKED_REGISTERS{8}; + uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS] {}; + uint8_t _checked_next{0}; + +}; diff --git a/src/drivers/imu/lsm303d/lsm303d_main.cpp b/src/drivers/imu/lsm303d/lsm303d_main.cpp new file mode 100644 index 000000000000..87a65e04c079 --- /dev/null +++ b/src/drivers/imu/lsm303d/lsm303d_main.cpp @@ -0,0 +1,182 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * + * 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. + * + ****************************************************************************/ + +/** + * @file lsm303d_main.cpp + * Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI. + */ + +#include "LSM303D.hpp" + +#include + +/** + * Local functions in support of the shell command. + */ +namespace lsm303d +{ + +LSM303D *g_dev; + +void start(bool external_bus, enum Rotation rotation, unsigned range); +void info(); +void usage(); + +/** + * Start the driver. + * + * This function call only returns once the driver is + * up and running or failed to detect the sensor. + */ +void +start(bool external_bus, enum Rotation rotation, unsigned range) +{ + if (g_dev != nullptr) { + errx(0, "already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG) + g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation); + } + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating LSM303D obj"); + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + exit(0); +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running\n"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +void +usage() +{ + PX4_INFO("missing command: try 'start', 'info'"); + PX4_INFO("options:"); + PX4_INFO(" -X (external bus)"); + PX4_INFO(" -R rotation"); +} + +} // namespace + +int +lsm303d_main(int argc, char *argv[]) +{ + bool external_bus = false; + enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + /* jump over start/off/etc and look at options first */ + while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + case 'a': + accel_range = atoi(myoptarg); + break; + + default: + lsm303d::usage(); + exit(0); + } + } + + if (myoptind >= argc) { + lsm303d::usage(); + exit(0); + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + + */ + if (!strcmp(verb, "start")) { + lsm303d::start(external_bus, rotation, accel_range); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + lsm303d::info(); + } + + errx(1, "unrecognized command, try 'start', info'"); +}