From eef325e2af17ff7047ff6cbd744ce256aa89aaae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 7 Jul 2018 10:58:52 -0400 Subject: [PATCH] bmi055 split accel and gyro headers --- src/drivers/imu/bmi055/BMI055.hpp | 109 +++ .../{bmi055_accel.cpp => BMI055_accel.cpp} | 79 ++- src/drivers/imu/bmi055/BMI055_accel.hpp | 298 ++++++++ .../{bmi055_gyro.cpp => BMI055_gyro.cpp} | 76 ++- src/drivers/imu/bmi055/BMI055_gyro.hpp | 301 +++++++++ src/drivers/imu/bmi055/CMakeLists.txt | 5 +- src/drivers/imu/bmi055/bmi055.hpp | 635 ------------------ src/drivers/imu/bmi055/bmi055_main.cpp | 40 +- 8 files changed, 833 insertions(+), 710 deletions(-) create mode 100644 src/drivers/imu/bmi055/BMI055.hpp rename src/drivers/imu/bmi055/{bmi055_accel.cpp => BMI055_accel.cpp} (90%) create mode 100644 src/drivers/imu/bmi055/BMI055_accel.hpp rename src/drivers/imu/bmi055/{bmi055_gyro.cpp => BMI055_gyro.cpp} (90%) create mode 100644 src/drivers/imu/bmi055/BMI055_gyro.hpp delete mode 100644 src/drivers/imu/bmi055/bmi055.hpp diff --git a/src/drivers/imu/bmi055/BMI055.hpp b/src/drivers/imu/bmi055/BMI055.hpp new file mode 100644 index 0000000000..e7b465717e --- /dev/null +++ b/src/drivers/imu/bmi055/BMI055.hpp @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +//Soft-reset command Value +#define BMI055_SOFT_RESET 0xB6 + +#define BMI055_BUS_SPEED 10*1000*1000 + +#define BMI055_TIMER_REDUCTION 200 + +class BMI055 : public device::SPI +{ + +protected: + + uint8_t _whoami; /** whoami result */ + + struct hrt_call _call; + unsigned _call_interval; + + unsigned _dlpf_freq; + + perf_counter_t _sample_perf; + perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; + perf_counter_t _good_transfers; + perf_counter_t _reset_retries; + perf_counter_t _duplicates; + + uint8_t _register_wait; + uint64_t _reset_wait; + + enum Rotation _rotation; + + uint8_t _checked_next; + + /** + * Read a register from the BMI055 + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + uint16_t read_reg16(unsigned reg); + + /** + * Write a register in the BMI055 + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /* do not allow to copy this class due to pointer data members */ + BMI055(const BMI055 &); + BMI055 operator=(const BMI055 &); + +public: + + BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency, + enum Rotation rotation); + + virtual ~BMI055(); + + +}; diff --git a/src/drivers/imu/bmi055/bmi055_accel.cpp b/src/drivers/imu/bmi055/BMI055_accel.cpp similarity index 90% rename from src/drivers/imu/bmi055/bmi055_accel.cpp rename to src/drivers/imu/bmi055/BMI055_accel.cpp index 690c695707..4fe1c2c88f 100644 --- a/src/drivers/imu/bmi055/bmi055_accel.cpp +++ b/src/drivers/imu/bmi055/BMI055_accel.cpp @@ -1,4 +1,38 @@ -#include "bmi055.hpp" +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#include "BMI055_accel.hpp" + #include /* @@ -12,8 +46,6 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER BMI055_ACC_INT_MAP_1, }; - - BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), _accel_reports(nullptr), @@ -32,9 +64,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu _last_temperature(0), _got_duplicate(false) { - // disable debug() calls - _debug_enabled = false; - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI055; // default accel scale factors @@ -48,7 +77,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu memset(&_call, 0, sizeof(_call)); } - BMI055_accel::~BMI055_accel() { /* make sure we are truly inactive */ @@ -59,7 +87,6 @@ BMI055_accel::~BMI055_accel() delete _accel_reports; } - if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); } @@ -68,14 +95,11 @@ BMI055_accel::~BMI055_accel() perf_free(_accel_reads); } - int BMI055_accel::init() { - int ret; - /* do SPI init (and probe) first */ - ret = SPI::init(); + int ret = SPI::init(); /* if probe/setup failed, bail now */ if (ret != OK) { @@ -102,7 +126,6 @@ BMI055_accel::init() _accel_scale.z_offset = 0; _accel_scale.z_scale = 1.0f; - _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); measure(); @@ -162,7 +185,6 @@ int BMI055_accel::reset() return OK; } - int BMI055_accel::probe() { @@ -183,15 +205,12 @@ BMI055_accel::probe() return -EIO; } - - int BMI055_accel::accel_set_sample_rate(float frequency) { uint8_t setbits = 0; uint8_t clearbits = BMI055_ACCEL_BW_1000; - if (frequency < (3125 / 100)) { setbits |= BMI055_ACCEL_BW_7_81; _accel_sample_rate = 1563 / 100; @@ -234,8 +253,6 @@ BMI055_accel::accel_set_sample_rate(float frequency) return OK; } - - ssize_t BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) { @@ -322,7 +339,6 @@ BMI055_accel::accel_self_test() return 0; } - /* deliberately trigger an error in the sensor to trigger recovery */ @@ -334,7 +350,6 @@ BMI055_accel::test_error() print_registers(); } - int BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) { @@ -364,8 +379,8 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_ACCEL_MAX_RATE); case SENSOR_POLLRATE_DEFAULT: - return ioctl(filp, SENSORIOCSPOLLRATE, - BMI055_ACCEL_DEFAULT_RATE); //Polling at the highest frequency. We may get duplicate values on the sensors + // Polling at the highest frequency. We may get duplicate values on the sensors + return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_ACCEL_DEFAULT_RATE); /* adjust to a legal polling interval in Hz */ default: { @@ -474,11 +489,10 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) } } - void BMI055_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; + uint8_t val; val = read_reg(reg); val &= ~clearbits; @@ -543,7 +557,6 @@ BMI055_accel::set_accel_range(unsigned max_g) return OK; } - void BMI055_accel::start() { @@ -619,7 +632,6 @@ BMI055_accel::check_registers(void) _checked_next = (_checked_next + 1) % BMI055_ACCEL_NUM_CHECKED_REGISTERS; } - void BMI055_accel::measure() { @@ -701,7 +713,6 @@ BMI055_accel::measure() return; } - perf_count(_good_transfers); if (_register_wait != 0) { @@ -715,12 +726,10 @@ BMI055_accel::measure() /* * Report buffers. */ - accel_report arb; - + accel_report arb; arb.timestamp = hrt_absolute_time(); - // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on @@ -791,11 +800,11 @@ BMI055_accel::measure() perf_end(_sample_perf); } - void BMI055_accel::print_info() { - warnx("BMI055 Accel"); + PX4_INFO("Accel"); + perf_print_counter(_sample_perf); perf_print_counter(_accel_reads); perf_print_counter(_bad_transfers); @@ -803,6 +812,7 @@ BMI055_accel::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); + _accel_reports->print_info("accel queue"); ::printf("checked_next: %u\n", _checked_next); @@ -828,7 +838,6 @@ BMI055_accel::print_info() printf("\n"); } - void BMI055_accel::print_registers() { @@ -861,5 +870,3 @@ BMI055_accel::print_registers() printf("\n"); } - - diff --git a/src/drivers/imu/bmi055/BMI055_accel.hpp b/src/drivers/imu/bmi055/BMI055_accel.hpp new file mode 100644 index 0000000000..709cf3783c --- /dev/null +++ b/src/drivers/imu/bmi055/BMI055_accel.hpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#pragma once + +#include "BMI055.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define BMI055_DEVICE_PATH_ACCEL "/dev/bmi055_accel" +#define BMI055_DEVICE_PATH_ACCEL_EXT "/dev/bmi055_accel_ext" + +// BMI055 Accel registers +#define BMI055_ACC_CHIP_ID 0x00 +#define BMI055_ACC_X_L 0x02 +#define BMI055_ACC_X_H 0x03 +#define BMI055_ACC_Y_L 0x04 +#define BMI055_ACC_Y_H 0x05 +#define BMI055_ACC_Z_L 0x06 +#define BMI055_ACC_Z_H 0x07 +#define BMI055_ACC_TEMP 0x08 +#define BMI055_ACC_INT_STATUS_0 0x09 +#define BMI055_ACC_INT_STATUS_1 0x0A +#define BMI055_ACC_INT_STATUS_2 0x0B +#define BMI055_ACC_INT_STATUS_3 0x0C +#define BMI055_ACC_FIFO_STATUS 0x0E +#define BMI055_ACC_RANGE 0x0F +#define BMI055_ACC_BW 0x10 +#define BMI055_ACC_PMU_LPW 0x11 +#define BMI055_ACC_PMU_LOW_POWER 0x12 +#define BMI055_ACC_DATA_CTRL 0x13 +#define BMI055_ACC_SOFTRESET 0x14 +#define BMI055_ACC_INT_EN_0 0x16 +#define BMI055_ACC_INT_EN_1 0x17 +#define BMI055_ACC_INT_EN_2 0x18 +#define BMI055_ACC_INT_MAP_0 0x19 +#define BMI055_ACC_INT_MAP_1 0x1A +#define BMI055_ACC_INT_MAP_2 0x1B +#define BMI055_ACC_INT_SRC 0x1E +#define BMI055_ACC_INT_OUT_CTRL 0x20 +#define BMI055_ACC_INT_LATCH 0x21 +#define BMI055_ACC_INT_LH_0 0x22 +#define BMI055_ACC_INT_LH_1 0x23 +#define BMI055_ACC_INT_LH_2 0x24 +#define BMI055_ACC_INT_LH_3 0x25 +#define BMI055_ACC_INT_LH_4 0x26 +#define BMI055_ACC_INT_MOT_0 0x27 +#define BMI055_ACC_INT_MOT_1 0x28 +#define BMI055_ACC_INT_MOT_2 0x29 +#define BMI055_ACC_INT_TAP_0 0x2A +#define BMI055_ACC_INT_TAP_1 0x2B +#define BMI055_ACC_INT_ORIE_0 0x2C +#define BMI055_ACC_INT_ORIE_1 0x2D +#define BMI055_ACC_INT_FLAT_0 0x2E +#define BMI055_ACC_INT_FLAT_1 0x2F +#define BMI055_ACC_FIFO_CONFIG_0 0x30 +#define BMI055_ACC_SELF_TEST 0x32 +#define BMI055_ACC_EEPROM_CTRL 0x33 +#define BMI055_ACC_SERIAL_CTRL 0x34 +#define BMI055_ACC_OFFSET_CTRL 0x36 +#define BMI055_ACC_OFC_SETTING 0x37 +#define BMI055_ACC_OFFSET_X 0x38 +#define BMI055_ACC_OFFSET_Y 0x39 +#define BMI055_ACC_OFFSET_Z 0x3A +#define BMI055_ACC_TRIM_GPO 0x3B +#define BMI055_ACC_TRIM_GP1 0x3C +#define BMI055_ACC_FIFO_CONFIG_1 0x3E +#define BMI055_ACC_FIFO_DATA 0x3F + +// BMI055 Accelerometer Chip-Id +#define BMI055_ACC_WHO_AM_I 0xFA + +//BMI055_ACC_BW 0x10 +#define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0) +#define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0) +#define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0) +#define BMI055_ACCEL_BW_62_5 (1<<3) | (0<<2) | (1<<1) | (1<<0) +#define BMI055_ACCEL_BW_125 (1<<3) | (1<<2) | (0<<1) | (0<<0) +#define BMI055_ACCEL_BW_250 (1<<3) | (1<<2) | (0<<1) | (1<<0) +#define BMI055_ACCEL_BW_500 (1<<3) | (1<<2) | (1<<1) | (0<<0) +#define BMI055_ACCEL_BW_1000 (1<<3) | (1<<2) | (1<<1) | (1<<0) + +//BMI055_ACC_PMU_LPW 0x11 +#define BMI055_ACCEL_NORMAL (0<<7) | (0<<6) | (0<<5) +#define BMI055_ACCEL_DEEP_SUSPEND (0<<7) | (0<<6) | (1<<5) +#define BMI055_ACCEL_LOW_POWER (0<<7) | (1<<6) | (0<<5) +#define BMI055_ACCEL_SUSPEND (1<<7) | (0<<6) | (0<<5) + +//BMI055_ACC_RANGE 0x0F +#define BMI055_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0) +#define BMI055_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0) +#define BMI055_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0) +#define BMI055_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0) + +//BMI055_ACC_INT_EN_1 0x17 +#define BMI055_ACC_DRDY_INT_EN (1<<4) + +//BMI055_ACC_INT_MAP_1 0x1A +#define BMI055_ACC_DRDY_INT1 (1<<0) + +// Default and Max values +#define BMI055_ACCEL_DEFAULT_RANGE_G 16 +#define BMI055_ACCEL_DEFAULT_RATE 1000 +#define BMI055_ACCEL_MAX_RATE 1000 +#define BMI055_ACCEL_MAX_PUBLISH_RATE 280 + +#define BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50 + +/* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */ +#define BMI055_NEW_DATA_MASK 0x01 + +class BMI055_accel : public BMI055 +{ +public: + BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation); + virtual ~BMI055_accel(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + + virtual int probe(); + +private: + + ringbuffer::RingBuffer *_accel_reports; + + struct accel_calibration_s _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + float _accel_sample_rate; + perf_counter_t _accel_reads; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + + Integrator _accel_int; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define BMI055_ACCEL_NUM_CHECKED_REGISTERS 5 + static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS]; + + // last temperature reading for print_info() + float _last_temperature; + + bool _got_duplicate; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Modify a register in the BMI055_accel + * + * 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 BMI055_accel, 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 BMI055_accel measurement range. + * + * @param max_g The maximum G value the range must support. + * @return OK if the value can be supported, -EINVAL otherwise. + */ + int set_accel_range(unsigned max_g); + + /** + * Measurement self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /* + * set accel sample rate + */ + int accel_set_sample_rate(float desired_sample_rate_hz); + + /* + * check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + BMI055_accel(const BMI055_accel &); + BMI055_accel operator=(const BMI055_accel &); + +}; diff --git a/src/drivers/imu/bmi055/bmi055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp similarity index 90% rename from src/drivers/imu/bmi055/bmi055_gyro.cpp rename to src/drivers/imu/bmi055/BMI055_gyro.cpp index 1774943317..22e79a1f84 100644 --- a/src/drivers/imu/bmi055/bmi055_gyro.cpp +++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp @@ -1,6 +1,37 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ -#include "bmi055.hpp" - +#include "BMI055_gyro.hpp" /* list of registers that will be checked in check_registers(). Note @@ -16,7 +47,6 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] BMI055_GYR_INT_MAP_1 }; - BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), _gyro_reports(nullptr), @@ -34,9 +64,6 @@ BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum R _gyro_int(1000000 / BMI055_GYRO_MAX_PUBLISH_RATE, true), _last_temperature(0) { - // disable debug() calls - _debug_enabled = false; - _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_BMI055; // default gyro scale factors @@ -50,7 +77,6 @@ BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum R memset(&_call, 0, sizeof(_call)); } - BMI055_gyro::~BMI055_gyro() { /* make sure we are truly inactive */ @@ -72,10 +98,8 @@ BMI055_gyro::~BMI055_gyro() int BMI055_gyro::init() { - int ret; - /* do SPI init (and probe) first */ - ret = SPI::init(); + int ret = SPI::init(); /* if probe/setup failed, bail now */ if (ret != OK) { @@ -84,7 +108,7 @@ BMI055_gyro::init() } /* allocate basic report buffers */ - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); if (_gyro_reports == nullptr) { goto out; @@ -128,7 +152,6 @@ out: return ret; } - int BMI055_gyro::reset() { write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset @@ -141,7 +164,6 @@ int BMI055_gyro::reset() set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR - //Enable Gyroscope in normal mode write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL); up_udelay(1000); @@ -188,7 +210,6 @@ BMI055_gyro::probe() return -EIO; } - int BMI055_gyro::gyro_set_sample_rate(float frequency) { @@ -220,7 +241,6 @@ BMI055_gyro::gyro_set_sample_rate(float frequency) return OK; } - int BMI055_gyro::self_test() { @@ -338,12 +358,14 @@ BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen) return (transferred * sizeof(gyro_report)); } - int BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCRESET: + return reset(); + case SENSORIOCSPOLLRATE: { switch (arg) { @@ -380,6 +402,7 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) return -EINVAL; } + // adjust filters float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f / ticks; _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); @@ -414,9 +437,6 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; - case SENSORIOCRESET: - return reset(); - case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) { @@ -466,8 +486,6 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) } } - - void BMI055_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) { @@ -492,7 +510,6 @@ BMI055_gyro::write_checked_reg(unsigned reg, uint8_t value) } } - int BMI055_gyro::set_gyro_range(unsigned max_dps) { @@ -617,7 +634,6 @@ BMI055_gyro::check_registers(void) _checked_next = (_checked_next + 1) % BMI055_GYRO_NUM_CHECKED_REGISTERS; } - void BMI055_gyro::measure() { @@ -685,10 +701,9 @@ BMI055_gyro::measure() /* * Report buffers. */ - gyro_report grb; + gyro_report grb; - - grb.timestamp = hrt_absolute_time(); + grb.timestamp = hrt_absolute_time(); // report the error count as the sum of the number of bad // transfers and bad register reads. This allows the higher @@ -764,7 +779,8 @@ BMI055_gyro::measure() void BMI055_gyro::print_info() { - warnx("BMI055 Gyro"); + PX4_INFO("Gyro"); + perf_print_counter(_sample_perf); perf_print_counter(_gyro_reads); perf_print_counter(_bad_transfers); @@ -772,6 +788,7 @@ BMI055_gyro::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); + _gyro_reports->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); @@ -797,7 +814,6 @@ BMI055_gyro::print_info() printf("\n"); } - void BMI055_gyro::print_registers() { @@ -840,7 +856,3 @@ BMI055_gyro::print_registers() printf("\n"); } - - - - diff --git a/src/drivers/imu/bmi055/BMI055_gyro.hpp b/src/drivers/imu/bmi055/BMI055_gyro.hpp new file mode 100644 index 0000000000..d3cd0d98ee --- /dev/null +++ b/src/drivers/imu/bmi055/BMI055_gyro.hpp @@ -0,0 +1,301 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#pragma once + +#include "BMI055.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define BMI055_DEVICE_PATH_GYRO "/dev/bmi055_gyro" +#define BMI055_DEVICE_PATH_GYRO_EXT "/dev/bmi055_gyro_ext" + +// BMI055 Gyro registers +#define BMI055_GYR_CHIP_ID 0x00 +#define BMI055_GYR_X_L 0x02 +#define BMI055_GYR_X_H 0x03 +#define BMI055_GYR_Y_L 0x04 +#define BMI055_GYR_Y_H 0x05 +#define BMI055_GYR_Z_L 0x06 +#define BMI055_GYR_Z_H 0x07 +#define BMI055_GYR_INT_STATUS_0 0x09 +#define BMI055_GYR_INT_STATUS_1 0x0A +#define BMI055_GYR_INT_STATUS_2 0x0B +#define BMI055_GYR_INT_STATUS_3 0x0C +#define BMI055_GYR_FIFO_STATUS 0x0E +#define BMI055_GYR_RANGE 0x0F +#define BMI055_GYR_BW 0x10 +#define BMI055_GYR_LPM1 0x11 +#define BMI055_GYR_LPM2 0x12 +#define BMI055_GYR_RATE_HBW 0x13 +#define BMI055_GYR_SOFTRESET 0x14 +#define BMI055_GYR_INT_EN_0 0x15 +#define BMI055_GYR_INT_EN_1 0x16 +#define BMI055_GYR_INT_MAP_0 0x17 +#define BMI055_GYR_INT_MAP_1 0x18 +#define BMI055_GYR_INT_MAP_2 0x19 +#define BMI055_GYRO_0_REG 0x1A +#define BMI055_GYRO_1_REG 0x1B +#define BMI055_GYRO_2_REG 0x1C +#define BMI055_GYRO_3_REG 0x1E +#define BMI055_GYR_INT_LATCH 0x21 +#define BMI055_GYR_INT_LH_0 0x22 +#define BMI055_GYR_INT_LH_1 0x23 +#define BMI055_GYR_INT_LH_2 0x24 +#define BMI055_GYR_INT_LH_3 0x25 +#define BMI055_GYR_INT_LH_4 0x26 +#define BMI055_GYR_INT_LH_5 0x27 +#define BMI055_GYR_SOC 0x31 +#define BMI055_GYR_A_FOC 0x32 +#define BMI055_GYR_TRIM_NVM_CTRL 0x33 +#define BMI055_BGW_SPI3_WDT 0x34 +#define BMI055_GYR_OFFSET_COMP 0x36 +#define BMI055_GYR_OFFSET_COMP_X 0x37 +#define BMI055_GYR_OFFSET_COMP_Y 0x38 +#define BMI055_GYR_OFFSET_COMP_Z 0x39 +#define BMI055_GYR_TRIM_GPO 0x3A +#define BMI055_GYR_TRIM_GP1 0x3B +#define BMI055_GYR_SELF_TEST 0x3C +#define BMI055_GYR_FIFO_CONFIG_0 0x3D +#define BMI055_GYR_FIFO_CONFIG_1 0x3E +#define BMI055_GYR_FIFO_DATA 0x3F + +// BMI055 Gyroscope Chip-Id +#define BMI055_GYR_WHO_AM_I 0x0F + +//BMI055_GYR_BW 0x10 +#define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0) +#define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0) +#define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0) +#define BMI055_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0) +#define BMI055_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0) + +//BMI055_GYR_LPM1 0x11 +#define BMI055_GYRO_NORMAL (0<<7) | (0<<5) +#define BMI055_GYRO_DEEP_SUSPEND (0<<7) | (1<<5) +#define BMI055_GYRO_SUSPEND (1<<7) | (0<<5) + +//BMI055_GYR_RANGE 0x0F +#define BMI055_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0) +#define BMI055_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0) +#define BMI055_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0) +#define BMI055_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0) +#define BMI055_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0) + +//BMI055_GYR_INT_EN_0 0x15 +#define BMI055_GYR_DRDY_INT_EN (1<<7) + +//BMI055_GYR_INT_MAP_1 0x18 +#define BMI055_GYR_DRDY_INT1 (1<<0) + +// Default and Max values +#define BMI055_GYRO_DEFAULT_RANGE_DPS 2000 +#define BMI055_GYRO_DEFAULT_RATE 1000 +#define BMI055_GYRO_MAX_RATE 1000 +#define BMI055_GYRO_MAX_PUBLISH_RATE 280 + +#define BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50 + +/* Mask definitions for Gyro bandwidth */ +#define BMI055_GYRO_BW_MASK 0x0F + +#define BMI055_ACC_TEMP 0x08 + +class BMI055_gyro : public BMI055 +{ +public: + BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation); + virtual ~BMI055_gyro(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + + void print_registers(); + + // deliberately cause a sensor error + void test_error(); + +protected: + + virtual int probe(); + +private: + + ringbuffer::RingBuffer *_gyro_reports; + + struct gyro_calibration_s _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + + orb_advert_t _gyro_topic; + int _gyro_orb_class_instance; + int _gyro_class_instance; + + float _gyro_sample_rate; + perf_counter_t _gyro_reads; + + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + + Integrator _gyro_int; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define BMI055_GYRO_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS]; + + // last temperature reading for print_info() + float _last_temperature; + + /** + * Start automatic measurement. + */ + void start(); + + /** + * Stop automatic measurement. + */ + void stop(); + + /** + * Reset chip. + * + * Resets the chip and measurements ranges, but not scale and offset. + */ + int reset(); + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Fetch measurements from the sensor and update the report buffers. + */ + void measure(); + + /** + * Modify a register in the BMI055_gyro + * + * 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 BMI055_gyro, 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 BMI055_gyro measurement range. + * + * @param max_dps The maximum DPS value the range must support. + * @return OK if the value can be supported, -EINVAL otherwise. + */ + int set_gyro_range(unsigned max_dps); + + /** + * Measurement self test + * + * @return 0 on success, 1 on failure + */ + int self_test(); + + /** + * Gyro self test + * + * @return 0 on success, 1 on failure + */ + int gyro_self_test(); + + /* + * set gyro sample rate + */ + int gyro_set_sample_rate(float desired_sample_rate_hz); + + /* + * check that key registers still have the right value + */ + void check_registers(void); + + /* do not allow to copy this class due to pointer data members */ + BMI055_gyro(const BMI055_gyro &); + BMI055_gyro operator=(const BMI055_gyro &); + +#pragma pack(push, 1) + /** + * Report conversation within the BMI055_gyro, including command byte and + * interrupt status. + */ + struct BMI_GyroReport { + uint8_t cmd; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + }; +#pragma pack(pop) + +}; diff --git a/src/drivers/imu/bmi055/CMakeLists.txt b/src/drivers/imu/bmi055/CMakeLists.txt index 7c52f4896f..bb79feb640 100644 --- a/src/drivers/imu/bmi055/CMakeLists.txt +++ b/src/drivers/imu/bmi055/CMakeLists.txt @@ -36,9 +36,8 @@ px4_add_module( STACK_MAIN 1200 COMPILE_FLAGS SRCS - bmi055_accel.cpp - bmi055_gyro.cpp + BMI055_accel.cpp + BMI055_gyro.cpp bmi055_main.cpp - DEPENDS ) diff --git a/src/drivers/imu/bmi055/bmi055.hpp b/src/drivers/imu/bmi055/bmi055.hpp deleted file mode 100644 index 884cf03efe..0000000000 --- a/src/drivers/imu/bmi055/bmi055.hpp +++ /dev/null @@ -1,635 +0,0 @@ -#ifndef BMI055_HPP_ -#define BMI055_HPP_ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -#define BMI055_DEVICE_PATH_ACCEL "/dev/bmi055_accel" -#define BMI055_DEVICE_PATH_GYRO "/dev/bmi055_gyro" - -#define BMI055_DEVICE_PATH_ACCEL_EXT "/dev/bmi055_accel_ext" -#define BMI055_DEVICE_PATH_GYRO_EXT "/dev/bmi055_gyro_ext" - -// BMI055 Accel registers - -#define BMI055_ACC_CHIP_ID 0x00 -#define BMI055_ACC_X_L 0x02 -#define BMI055_ACC_X_H 0x03 -#define BMI055_ACC_Y_L 0x04 -#define BMI055_ACC_Y_H 0x05 -#define BMI055_ACC_Z_L 0x06 -#define BMI055_ACC_Z_H 0x07 -#define BMI055_ACC_TEMP 0x08 -#define BMI055_ACC_INT_STATUS_0 0x09 -#define BMI055_ACC_INT_STATUS_1 0x0A -#define BMI055_ACC_INT_STATUS_2 0x0B -#define BMI055_ACC_INT_STATUS_3 0x0C -#define BMI055_ACC_FIFO_STATUS 0x0E -#define BMI055_ACC_RANGE 0x0F -#define BMI055_ACC_BW 0x10 -#define BMI055_ACC_PMU_LPW 0x11 -#define BMI055_ACC_PMU_LOW_POWER 0x12 -#define BMI055_ACC_DATA_CTRL 0x13 -#define BMI055_ACC_SOFTRESET 0x14 -#define BMI055_ACC_INT_EN_0 0x16 -#define BMI055_ACC_INT_EN_1 0x17 -#define BMI055_ACC_INT_EN_2 0x18 -#define BMI055_ACC_INT_MAP_0 0x19 -#define BMI055_ACC_INT_MAP_1 0x1A -#define BMI055_ACC_INT_MAP_2 0x1B -#define BMI055_ACC_INT_SRC 0x1E -#define BMI055_ACC_INT_OUT_CTRL 0x20 -#define BMI055_ACC_INT_LATCH 0x21 -#define BMI055_ACC_INT_LH_0 0x22 -#define BMI055_ACC_INT_LH_1 0x23 -#define BMI055_ACC_INT_LH_2 0x24 -#define BMI055_ACC_INT_LH_3 0x25 -#define BMI055_ACC_INT_LH_4 0x26 -#define BMI055_ACC_INT_MOT_0 0x27 -#define BMI055_ACC_INT_MOT_1 0x28 -#define BMI055_ACC_INT_MOT_2 0x29 -#define BMI055_ACC_INT_TAP_0 0x2A -#define BMI055_ACC_INT_TAP_1 0x2B -#define BMI055_ACC_INT_ORIE_0 0x2C -#define BMI055_ACC_INT_ORIE_1 0x2D -#define BMI055_ACC_INT_FLAT_0 0x2E -#define BMI055_ACC_INT_FLAT_1 0x2F -#define BMI055_ACC_FIFO_CONFIG_0 0x30 -#define BMI055_ACC_SELF_TEST 0x32 -#define BMI055_ACC_EEPROM_CTRL 0x33 -#define BMI055_ACC_SERIAL_CTRL 0x34 -#define BMI055_ACC_OFFSET_CTRL 0x36 -#define BMI055_ACC_OFC_SETTING 0x37 -#define BMI055_ACC_OFFSET_X 0x38 -#define BMI055_ACC_OFFSET_Y 0x39 -#define BMI055_ACC_OFFSET_Z 0x3A -#define BMI055_ACC_TRIM_GPO 0x3B -#define BMI055_ACC_TRIM_GP1 0x3C -#define BMI055_ACC_FIFO_CONFIG_1 0x3E -#define BMI055_ACC_FIFO_DATA 0x3F - - -// BMI055 Gyro registers - -#define BMI055_GYR_CHIP_ID 0x00 -#define BMI055_GYR_X_L 0x02 -#define BMI055_GYR_X_H 0x03 -#define BMI055_GYR_Y_L 0x04 -#define BMI055_GYR_Y_H 0x05 -#define BMI055_GYR_Z_L 0x06 -#define BMI055_GYR_Z_H 0x07 -#define BMI055_GYR_INT_STATUS_0 0x09 -#define BMI055_GYR_INT_STATUS_1 0x0A -#define BMI055_GYR_INT_STATUS_2 0x0B -#define BMI055_GYR_INT_STATUS_3 0x0C -#define BMI055_GYR_FIFO_STATUS 0x0E -#define BMI055_GYR_RANGE 0x0F -#define BMI055_GYR_BW 0x10 -#define BMI055_GYR_LPM1 0x11 -#define BMI055_GYR_LPM2 0x12 -#define BMI055_GYR_RATE_HBW 0x13 -#define BMI055_GYR_SOFTRESET 0x14 -#define BMI055_GYR_INT_EN_0 0x15 -#define BMI055_GYR_INT_EN_1 0x16 -#define BMI055_GYR_INT_MAP_0 0x17 -#define BMI055_GYR_INT_MAP_1 0x18 -#define BMI055_GYR_INT_MAP_2 0x19 -#define BMI055_GYRO_0_REG 0x1A -#define BMI055_GYRO_1_REG 0x1B -#define BMI055_GYRO_2_REG 0x1C -#define BMI055_GYRO_3_REG 0x1E -#define BMI055_GYR_INT_LATCH 0x21 -#define BMI055_GYR_INT_LH_0 0x22 -#define BMI055_GYR_INT_LH_1 0x23 -#define BMI055_GYR_INT_LH_2 0x24 -#define BMI055_GYR_INT_LH_3 0x25 -#define BMI055_GYR_INT_LH_4 0x26 -#define BMI055_GYR_INT_LH_5 0x27 -#define BMI055_GYR_SOC 0x31 -#define BMI055_GYR_A_FOC 0x32 -#define BMI055_GYR_TRIM_NVM_CTRL 0x33 -#define BMI055_BGW_SPI3_WDT 0x34 -#define BMI055_GYR_OFFSET_COMP 0x36 -#define BMI055_GYR_OFFSET_COMP_X 0x37 -#define BMI055_GYR_OFFSET_COMP_Y 0x38 -#define BMI055_GYR_OFFSET_COMP_Z 0x39 -#define BMI055_GYR_TRIM_GPO 0x3A -#define BMI055_GYR_TRIM_GP1 0x3B -#define BMI055_GYR_SELF_TEST 0x3C -#define BMI055_GYR_FIFO_CONFIG_0 0x3D -#define BMI055_GYR_FIFO_CONFIG_1 0x3E -#define BMI055_GYR_FIFO_DATA 0x3F - - -// BMI055 Accelerometer Chip-Id -#define BMI055_ACC_WHO_AM_I 0xFA - -// BMI055 Gyroscope Chip-Id -#define BMI055_GYR_WHO_AM_I 0x0F - - - -//BMI055_ACC_BW 0x10 -#define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0) -#define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0) -#define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0) -#define BMI055_ACCEL_BW_62_5 (1<<3) | (0<<2) | (1<<1) | (1<<0) -#define BMI055_ACCEL_BW_125 (1<<3) | (1<<2) | (0<<1) | (0<<0) -#define BMI055_ACCEL_BW_250 (1<<3) | (1<<2) | (0<<1) | (1<<0) -#define BMI055_ACCEL_BW_500 (1<<3) | (1<<2) | (1<<1) | (0<<0) -#define BMI055_ACCEL_BW_1000 (1<<3) | (1<<2) | (1<<1) | (1<<0) - -//BMI055_ACC_PMU_LPW 0x11 -#define BMI055_ACCEL_NORMAL (0<<7) | (0<<6) | (0<<5) -#define BMI055_ACCEL_DEEP_SUSPEND (0<<7) | (0<<6) | (1<<5) -#define BMI055_ACCEL_LOW_POWER (0<<7) | (1<<6) | (0<<5) -#define BMI055_ACCEL_SUSPEND (1<<7) | (0<<6) | (0<<5) - - -//BMI055_ACC_RANGE 0x0F -#define BMI055_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0) -#define BMI055_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0) -#define BMI055_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0) -#define BMI055_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0) - -//BMI055_GYR_BW 0x10 -#define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0) -#define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0) -#define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0) -#define BMI055_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0) -#define BMI055_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0) - -//BMI055_GYR_LPM1 0x11 -#define BMI055_GYRO_NORMAL (0<<7) | (0<<5) -#define BMI055_GYRO_DEEP_SUSPEND (0<<7) | (1<<5) -#define BMI055_GYRO_SUSPEND (1<<7) | (0<<5) - -//BMI055_GYR_RANGE 0x0F -#define BMI055_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0) -#define BMI055_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0) -#define BMI055_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0) -#define BMI055_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0) -#define BMI055_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0) - - -//BMI055_ACC_INT_EN_1 0x17 -#define BMI055_ACC_DRDY_INT_EN (1<<4) - -//BMI055_GYR_INT_EN_0 0x15 -#define BMI055_GYR_DRDY_INT_EN (1<<7) - -//BMI055_ACC_INT_MAP_1 0x1A -#define BMI055_ACC_DRDY_INT1 (1<<0) - -//BMI055_GYR_INT_MAP_1 0x18 -#define BMI055_GYR_DRDY_INT1 (1<<0) - - - -//Soft-reset command Value -#define BMI055_SOFT_RESET 0xB6 - -// Default and Max values -#define BMI055_ACCEL_DEFAULT_RANGE_G 16 -#define BMI055_GYRO_DEFAULT_RANGE_DPS 2000 -#define BMI055_ACCEL_DEFAULT_RATE 1000 -#define BMI055_ACCEL_MAX_RATE 1000 -#define BMI055_ACCEL_MAX_PUBLISH_RATE 280 -#define BMI055_GYRO_DEFAULT_RATE 1000 -#define BMI055_GYRO_MAX_RATE 1000 -#define BMI055_GYRO_MAX_PUBLISH_RATE BMI055_ACCEL_MAX_PUBLISH_RATE - -#define BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50 - -#define BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50 - -#define BMI055_BUS_SPEED 10*1000*1000 - -#define BMI055_TIMER_REDUCTION 200 - -/* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */ -#define BMI055_NEW_DATA_MASK 0x01 - -/* Mask definitions for Gyro bandwidth */ -#define BMI055_GYRO_BW_MASK 0x0F - -class BMI055 : public device::SPI -{ - -protected: - - uint8_t _whoami; /** whoami result */ - - struct hrt_call _call; - unsigned _call_interval; - - - - unsigned _dlpf_freq; - - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _bad_registers; - perf_counter_t _good_transfers; - perf_counter_t _reset_retries; - perf_counter_t _duplicates; - - uint8_t _register_wait; - uint64_t _reset_wait; - - enum Rotation _rotation; - - uint8_t _checked_next; - - /** - * Read a register from the BMI055 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - uint16_t read_reg16(unsigned reg); - - /** - * Write a register in the BMI055 - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /* do not allow to copy this class due to pointer data members */ - BMI055(const BMI055 &); - BMI055 operator=(const BMI055 &); - -public: - - BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency, - enum Rotation rotation); - virtual ~BMI055(); - - -}; - - - -class BMI055_accel : public BMI055 -{ -public: - BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation); - virtual ~BMI055_accel(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - void print_registers(); - - // deliberately cause a sensor error - void test_error(); - -protected: - virtual int probe(); - - -private: - - ringbuffer::RingBuffer *_accel_reports; - - struct accel_calibration_s _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - int _accel_orb_class_instance; - int _accel_class_instance; - - - - float _accel_sample_rate; - perf_counter_t _accel_reads; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - - Integrator _accel_int; - - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define BMI055_ACCEL_NUM_CHECKED_REGISTERS 5 - static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS]; - uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS]; - - - // last temperature reading for print_info() - float _last_temperature; - - bool _got_duplicate; - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - void measure(); - - - /** - * Modify a register in the BMI055_accel - * - * 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 BMI055_accel, 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 BMI055_accel measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -EINVAL otherwise. - */ - int set_accel_range(unsigned max_g); - - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - - /** - * Accel self test - * - * @return 0 on success, 1 on failure - */ - int accel_self_test(); - - /* - set accel sample rate - */ - int accel_set_sample_rate(float desired_sample_rate_hz); - - /* - check that key registers still have the right value - */ - void check_registers(void); - - /* do not allow to copy this class due to pointer data members */ - BMI055_accel(const BMI055_accel &); - BMI055_accel operator=(const BMI055_accel &); - -}; - - - -class BMI055_gyro : public BMI055 -{ -public: - BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation); - virtual ~BMI055_gyro(); - - virtual int init(); - - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - void print_registers(); - - // deliberately cause a sensor error - void test_error(); - -protected: - virtual int probe(); -private: - - ringbuffer::RingBuffer *_gyro_reports; - - struct gyro_calibration_s _gyro_scale; - float _gyro_range_scale; - float _gyro_range_rad_s; - - orb_advert_t _gyro_topic; - int _gyro_orb_class_instance; - int _gyro_class_instance; - - - - float _gyro_sample_rate; - perf_counter_t _gyro_reads; - - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - - Integrator _gyro_int; - - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset -#define BMI055_GYRO_NUM_CHECKED_REGISTERS 7 - static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS]; - uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS]; - - - // last temperature reading for print_info() - float _last_temperature; - - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - /** - * Static trampoline from the hrt_call context; because we don't have a - * generic hrt wrapper yet. - * - * Called by the HRT in interrupt context at the specified rate if - * automatic polling is enabled. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void measure_trampoline(void *arg); - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - void measure(); - - - /** - * Modify a register in the BMI055_gyro - * - * 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 BMI055_gyro, 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 BMI055_gyro measurement range. - * - * @param max_dps The maximum DPS value the range must support. - * @return OK if the value can be supported, -EINVAL otherwise. - */ - int set_gyro_range(unsigned max_dps); - - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - - - /** - * Gyro self test - * - * @return 0 on success, 1 on failure - */ - int gyro_self_test(); - - - /* - * set gyro sample rate - */ - int gyro_set_sample_rate(float desired_sample_rate_hz); - - /* - * check that key registers still have the right value - */ - void check_registers(void); - - /* do not allow to copy this class due to pointer data members */ - BMI055_gyro(const BMI055_gyro &); - BMI055_gyro operator=(const BMI055_gyro &); - -#pragma pack(push, 1) - /** - * Report conversation within the BMI055_gyro, including command byte and - * interrupt status. - */ - struct BMI_GyroReport { - uint8_t cmd; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - }; - -#pragma pack(pop) -}; - - - -#endif /* BMI055_HPP_ */ diff --git a/src/drivers/imu/bmi055/bmi055_main.cpp b/src/drivers/imu/bmi055/bmi055_main.cpp index fbbf2a2a4f..3713c44b6c 100644 --- a/src/drivers/imu/bmi055/bmi055_main.cpp +++ b/src/drivers/imu/bmi055/bmi055_main.cpp @@ -1,6 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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. + * + ****************************************************************************/ + +#include "BMI055_accel.hpp" +#include "BMI055_gyro.hpp" + #include #include -#include "bmi055.hpp" /** driver 'main' command */ extern "C" { __EXPORT int bmi055_main(int argc, char *argv[]); } @@ -15,7 +50,6 @@ enum sensor_type { BMI055_GYRO }; - namespace bmi055 { @@ -24,7 +58,6 @@ BMI055_accel *g_acc_dev_ext; // on external bus (accel) BMI055_gyro *g_gyr_dev_int; // on internal bus (gyro) BMI055_gyro *g_gyr_dev_ext; // on external bus (gyro) - void start(bool, enum Rotation, enum sensor_type); void stop(bool, enum sensor_type); void test(bool, enum sensor_type); @@ -51,7 +84,6 @@ start(bool external_bus, enum Rotation rotation, enum sensor_type sensor) BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO; - if (sensor == BMI055_ACCEL) { if (*g_dev_acc_ptr != nullptr) /* if already started, the still command succeeded */