From 9ae2f0ea969a4808fc5d2e554b812e353e8dde8d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 1 Apr 2020 20:30:14 -0400 Subject: [PATCH] bmi160: cleanup --- src/drivers/drv_sensor.h | 4 +- src/drivers/imu/CMakeLists.txt | 3 + src/drivers/imu/bmi160/bmi160.cpp | 179 ++++------------------ src/drivers/imu/bmi160/bmi160.hpp | 48 +----- src/drivers/imu/bmi160/bmi160_main.cpp | 33 ---- src/drivers/imu/invensense/CMakeLists.txt | 41 +++++ 6 files changed, 84 insertions(+), 224 deletions(-) create mode 100644 src/drivers/imu/invensense/CMakeLists.txt diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 88cdfbf00c..9afdeb0c66 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -69,13 +69,13 @@ #define DRV_ACC_DEVTYPE_MPU6000_LEGACY 0x13 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14 #define DRV_ACC_DEVTYPE_MPU9250_LEGACY 0x16 -#define DRV_ACC_DEVTYPE_BMI160 0x17 +#define DRV_IMU_DEVTYPE_BMI160 0x17 #define DRV_IMU_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_L3GD20 0x22 #define DRV_GYR_DEVTYPE_GYROSIM 0x23 #define DRV_IMU_DEVTYPE_MPU9250 0x24 -#define DRV_GYR_DEVTYPE_BMI160 0x25 + #define DRV_IMU_DEVTYPE_ICM42688P 0x26 #define DRV_IMU_DEVTYPE_ICM40609D 0x27 diff --git a/src/drivers/imu/CMakeLists.txt b/src/drivers/imu/CMakeLists.txt index 2b12572fd5..54947f6dd5 100644 --- a/src/drivers/imu/CMakeLists.txt +++ b/src/drivers/imu/CMakeLists.txt @@ -36,11 +36,14 @@ add_subdirectory(adis16477) add_subdirectory(adis16497) add_subdirectory(bma180) add_subdirectory(bmi055) +add_subdirectory(bmi088) add_subdirectory(bmi160) add_subdirectory(fxas21002c) add_subdirectory(fxos8701cq) add_subdirectory(icm20948) +add_subdirectory(invensense) add_subdirectory(l3gd20) add_subdirectory(lsm303d) add_subdirectory(mpu6000) add_subdirectory(mpu9250) +add_subdirectory(st) diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 2e55d1de3b..23b283ff58 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -53,16 +53,16 @@ BMI160::BMI160(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio spi_mode_e spi_mode) : SPI(DRV_IMU_DEVTYPE_BMI160, MODULE_NAME, bus, device, spi_mode, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), - _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation), - _accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")), - _gyro_reads(perf_alloc(PC_COUNT, "bmi160_gyro_read")), - _sample_perf(perf_alloc(PC_ELAPSED, "bmi160_read")), - _bad_transfers(perf_alloc(PC_COUNT, "bmi160_bad_transfers")), - _bad_registers(perf_alloc(PC_COUNT, "bmi160_bad_registers")), - _good_transfers(perf_alloc(PC_COUNT, "bmi160_good_transfers")), - _reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")), - _duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates")) + _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _accel_reads(perf_alloc(PC_COUNT, MODULE_NAME": accel read")), + _gyro_reads(perf_alloc(PC_COUNT, MODULE_NAME":gyro read")), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME":read")), + _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME":bad transfers")), + _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME":bad registers")), + _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME":good transfers")), + _reset_retries(perf_alloc(PC_COUNT, MODULE_NAME":reset retries")), + _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates")) { } @@ -79,8 +79,7 @@ BMI160::~BMI160() perf_free(_duplicates); } -int -BMI160::init() +int BMI160::init() { /* do SPI init (and probe) first */ int ret = SPI::init(); @@ -123,18 +122,13 @@ int BMI160::reset() set_gyro_range(BMI160_GYRO_DEFAULT_RANGE_DPS); gyro_set_sample_rate(BMI160_GYRO_DEFAULT_RATE); - - //_set_dlpf_filter(BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); //NOT CONSIDERING FILTERING YET - - //Enable Accelerometer in normal mode + // Enable Accelerometer in normal mode write_reg(BMIREG_CMD, BMI_ACCEL_NORMAL_MODE); - up_udelay(4100); - //usleep(4100); + px4_usleep(4100); //Enable Gyroscope in normal mode write_reg(BMIREG_CMD, BMI_GYRO_NORMAL_MODE); - up_udelay(80300); - //usleep(80300); + px4_usleep(80300); uint8_t retries = 10; @@ -159,8 +153,7 @@ int BMI160::reset() return OK; } -int -BMI160::probe() +int BMI160::probe() { /* look for device ID */ _whoami = read_reg(BMIREG_CHIP_ID); @@ -179,8 +172,7 @@ BMI160::probe() return -EIO; } -int -BMI160::accel_set_sample_rate(float frequency) +int BMI160::accel_set_sample_rate(float frequency) { uint8_t setbits = 0; uint8_t clearbits = (BMI_ACCEL_RATE_25_8 | BMI_ACCEL_RATE_1600); @@ -246,8 +238,7 @@ BMI160::accel_set_sample_rate(float frequency) return OK; } -int -BMI160::gyro_set_sample_rate(float frequency) +int BMI160::gyro_set_sample_rate(float frequency) { uint8_t setbits = 0; uint8_t clearbits = (BMI_GYRO_RATE_200 | BMI_GYRO_RATE_25); @@ -297,95 +288,31 @@ BMI160::gyro_set_sample_rate(float frequency) return OK; } -void -BMI160::_set_dlpf_filter(uint16_t bandwidth) -{ - _dlpf_freq = 0; - bandwidth = bandwidth; //TO BE IMPLEMENTED - /*uint8_t setbits = BW_SCAL_ODR_BW_XL; - uint8_t clearbits = BW_XL_50_HZ; - - if (bandwidth == 0) { - _dlpf_freq = 408; - clearbits = BW_SCAL_ODR_BW_XL | BW_XL_50_HZ; - setbits = 0; - } - - if (bandwidth <= 50) { - setbits |= BW_XL_50_HZ; - _dlpf_freq = 50; - - } else if (bandwidth <= 105) { - setbits |= BW_XL_105_HZ; - _dlpf_freq = 105; - - } else if (bandwidth <= 211) { - setbits |= BW_XL_211_HZ; - _dlpf_freq = 211; - - } else if (bandwidth <= 408) { - setbits |= BW_XL_408_HZ; - _dlpf_freq = 408; - - } - modify_reg(CTRL_REG6_XL, clearbits, setbits);*/ -} - -/* - deliberately trigger an error in the sensor to trigger recovery - */ -void -BMI160::test_error() -{ - write_reg(BMIREG_CMD, BMI160_SOFT_RESET); - ::printf("error triggered\n"); - print_registers(); -} - -uint8_t -BMI160::read_reg(unsigned reg) +uint8_t BMI160::read_reg(uint8_t reg) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - transfer(cmd, cmd, sizeof(cmd)); - return cmd[1]; } -uint16_t -BMI160::read_reg16(unsigned reg) -{ - uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - - transfer(cmd, cmd, sizeof(cmd)); - - return (uint16_t)(cmd[1] << 8) | cmd[2]; -} - -void -BMI160::write_reg(unsigned reg, uint8_t value) +void BMI160::write_reg(uint8_t reg, uint8_t value) { uint8_t cmd[2]; - cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); } void -BMI160::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +BMI160::modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits) { - uint8_t val; - - val = read_reg(reg); + uint8_t val = read_reg(reg); val &= ~clearbits; val |= setbits; write_checked_reg(reg, val); } -void -BMI160::write_checked_reg(unsigned reg, uint8_t value) +void BMI160::write_checked_reg(uint8_t reg, uint8_t value) { write_reg(reg, value); @@ -397,8 +324,7 @@ BMI160::write_checked_reg(unsigned reg, uint8_t value) } } -int -BMI160::set_accel_range(unsigned max_g) +int BMI160::set_accel_range(unsigned max_g) { uint8_t setbits = 0; uint8_t clearbits = BMI_ACCEL_RANGE_2_G | BMI_ACCEL_RANGE_16_G; @@ -439,8 +365,7 @@ BMI160::set_accel_range(unsigned max_g) return OK; } -int -BMI160::set_gyro_range(unsigned max_dps) +int BMI160::set_gyro_range(unsigned max_dps) { uint8_t setbits = 0; uint8_t clearbits = BMI_GYRO_RANGE_125_DPS | BMI_GYRO_RANGE_250_DPS; @@ -487,8 +412,7 @@ BMI160::set_gyro_range(unsigned max_dps) return OK; } -void -BMI160::start() +void BMI160::start() { /* start polling at the specified rate */ ScheduleOnInterval((1_s / BMI160_GYRO_DEFAULT_RATE) - BMI160_TIMER_REDUCTION, 1000); @@ -496,8 +420,7 @@ BMI160::start() reset(); } -void -BMI160::check_registers(void) +void BMI160::check_registers() { uint8_t v; @@ -539,8 +462,7 @@ BMI160::check_registers(void) _checked_next = (_checked_next + 1) % BMI160_NUM_CHECKED_REGISTERS; } -void -BMI160::RunImpl() +void BMI160::RunImpl() { if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete @@ -557,7 +479,7 @@ BMI160::RunImpl() int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; - } report; + } report{}; /* start measuring */ perf_begin(_sample_perf); @@ -567,7 +489,7 @@ BMI160::RunImpl() */ bmi_report.cmd = BMIREG_GYR_X_L | DIR_READ; - uint8_t status = read_reg(BMIREG_STATUS); + uint8_t status = read_reg(BMIREG_STATUS); const hrt_abstime timestamp_sample = hrt_absolute_time(); @@ -594,8 +516,6 @@ BMI160::RunImpl() report.temp = ((temp_h << 8) + temp_l); - - report.accel_x = bmi_report.accel_x; report.accel_y = bmi_report.accel_y; report.accel_z = bmi_report.accel_z; @@ -669,8 +589,7 @@ BMI160::RunImpl() perf_end(_sample_perf); } -void -BMI160::print_status() +void BMI160::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); @@ -681,44 +600,6 @@ BMI160::print_status() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - - ::printf("checked_next: %u\n", _checked_next); - - for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) { - uint8_t v = read_reg(_checked_registers[i]); - - if (v != _checked_values[i]) { - ::printf("reg %02x:%02x should be %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_values[i]); - } - - if (v != _checked_bad[i]) { - ::printf("reg %02x:%02x was bad %02x\n", - (unsigned)_checked_registers[i], - (unsigned)v, - (unsigned)_checked_bad[i]); - } - } - _px4_accel.print_status(); _px4_gyro.print_status(); } - -void -BMI160::print_registers() -{ - printf("BMI160 registers\n"); - - for (uint8_t reg = 0x40; reg <= 0x47; reg++) { - uint8_t v = read_reg(reg); - printf("%02x:%02x ", (unsigned)reg, (unsigned)v); - - if (reg % 13 == 0) { - printf("\n"); - } - } - - printf("\n"); -} diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index f5b6c88512..50e6e96aac 100644 --- a/src/drivers/imu/bmi160/bmi160.hpp +++ b/src/drivers/imu/bmi160/bmi160.hpp @@ -231,12 +231,6 @@ #define BMI160_GYRO_MAX_RATE 3200 #define BMI160_GYRO_MAX_PUBLISH_RATE BMI160_ACCEL_MAX_PUBLISH_RATE -#define BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 324 -#define BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50 - -#define BMI160_GYRO_DEFAULT_ONCHIP_FILTER_FREQ 254.6f -#define BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50 - #define BMI160_BUS_SPEED 10*1000*1000 #define BMI160_TIMER_REDUCTION 200 @@ -253,23 +247,12 @@ public: static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - int init() override; - - void print_status() override; - - void print_registers(); - - // deliberately cause a sensor error - void test_error(); - - void RunImpl(); - -protected: - int probe() override; - void custom_method(const BusCLIArguments &cli) override; + void print_status() override; + void RunImpl(); private: + int probe() override; PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; @@ -324,8 +307,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); - uint16_t read_reg16(unsigned reg); + uint8_t read_reg(uint8_t reg); /** * Write a register in the BMI160 @@ -333,7 +315,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + void write_reg(uint8_t reg, uint8_t value); /** * Modify a register in the BMI160 @@ -344,7 +326,7 @@ private: * @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); + void modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits); /** * Write a register in the BMI160, updating _checked_values @@ -352,7 +334,7 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_checked_reg(unsigned reg, uint8_t value); + void write_checked_reg(uint8_t reg, uint8_t value); /** * Set the BMI160 measurement range. @@ -364,16 +346,6 @@ private: int set_accel_range(unsigned max_g); int set_gyro_range(unsigned max_dps); - /** - * Swap a 16-bit value read from the BMI160 to native byte order. - */ - uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } - - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); - /* set sample rate (approximate) - 10 - 952 Hz */ @@ -382,11 +354,7 @@ private: /* 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 */ - BMI160(const BMI160 &); - BMI160 operator=(const BMI160 &); + void check_registers(); #pragma pack(push, 1) /** diff --git a/src/drivers/imu/bmi160/bmi160_main.cpp b/src/drivers/imu/bmi160/bmi160_main.cpp index 5242ddb3a7..2cafc719bc 100644 --- a/src/drivers/imu/bmi160/bmi160_main.cpp +++ b/src/drivers/imu/bmi160/bmi160_main.cpp @@ -44,9 +44,6 @@ BMI160::print_usage() PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - PRINT_MODULE_USAGE_COMMAND("reset"); - PRINT_MODULE_USAGE_COMMAND("regdump"); - PRINT_MODULE_USAGE_COMMAND("testerror"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -69,21 +66,6 @@ I2CSPIDriverBase *BMI160::instantiate(const BusCLIArguments &cli, const BusInsta return instance; } -void -BMI160::custom_method(const BusCLIArguments &cli) -{ - switch (cli.custom1) { - case 0: reset(); - break; - - case 1: print_registers(); - break; - - case 2: test_error(); - break; - } -} - extern "C" int bmi160_main(int argc, char *argv[]) { int ch; @@ -120,21 +102,6 @@ extern "C" int bmi160_main(int argc, char *argv[]) return ThisDriver::module_status(iterator); } - if (!strcmp(verb, "reset")) { - cli.custom1 = 0; - return ThisDriver::module_custom_method(cli, iterator); - } - - if (!strcmp(verb, "regdump")) { - cli.custom1 = 1; - return ThisDriver::module_custom_method(cli, iterator); - } - - if (!strcmp(verb, "testerror")) { - cli.custom1 = 2; - return ThisDriver::module_custom_method(cli, iterator); - } - ThisDriver::print_usage(); return -1; } diff --git a/src/drivers/imu/invensense/CMakeLists.txt b/src/drivers/imu/invensense/CMakeLists.txt new file mode 100644 index 0000000000..83f6c995da --- /dev/null +++ b/src/drivers/imu/invensense/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ + +add_subdirectory(icm20602) +add_subdirectory(icm20608g) +add_subdirectory(icm20689) +add_subdirectory(icm40609d) +add_subdirectory(icm42688p) +add_subdirectory(mpu6000) +add_subdirectory(mpu6500) +add_subdirectory(mpu9250)