From 11ad41f7cb920eb1f072e398dafb48f0202ab2f0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Feb 2021 14:46:08 -0500 Subject: [PATCH] delete old imu/mpu6000 driver --- boards/holybro/pix32v5/default.cmake | 1 - boards/nxp/fmurt1062-v1/default.cmake | 1 - boards/px4/fmu-v2/default.cmake | 1 - boards/px4/fmu-v2/multicopter.cmake | 2 +- boards/px4/fmu-v5/ctrlalloc.cmake | 1 - boards/px4/fmu-v5/debug.cmake | 1 - boards/px4/fmu-v5/default.cmake | 1 - boards/px4/fmu-v5/fixedwing.cmake | 1 - boards/px4/fmu-v5/multicopter.cmake | 1 - boards/px4/fmu-v5/optimized.cmake | 1 - boards/px4/fmu-v5/rover.cmake | 1 - boards/px4/fmu-v5/rtps.cmake | 1 - boards/px4/fmu-v5/stackcheck.cmake | 1 - boards/px4/fmu-v5/uavcanv0periph.cmake | 1 - boards/px4/fmu-v5/uavcanv1.cmake | 1 - boards/spracing/h7extreme/default.cmake | 2 +- cmake/px4_add_board.cmake | 4 +- src/drivers/imu/mpu6000/CMakeLists.txt | 44 -- src/drivers/imu/mpu6000/MPU6000.cpp | 903 ----------------------- src/drivers/imu/mpu6000/MPU6000.hpp | 455 ------------ src/drivers/imu/mpu6000/MPU6000_I2C.cpp | 123 --- src/drivers/imu/mpu6000/MPU6000_SPI.cpp | 210 ------ src/drivers/imu/mpu6000/mpu6000_main.cpp | 213 ------ 23 files changed, 4 insertions(+), 1966 deletions(-) delete mode 100644 src/drivers/imu/mpu6000/CMakeLists.txt delete mode 100644 src/drivers/imu/mpu6000/MPU6000.cpp delete mode 100644 src/drivers/imu/mpu6000/MPU6000.hpp delete mode 100644 src/drivers/imu/mpu6000/MPU6000_I2C.cpp delete mode 100644 src/drivers/imu/mpu6000/MPU6000_SPI.cpp delete mode 100644 src/drivers/imu/mpu6000/mpu6000_main.cpp diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 625c6cc5e4..7d92943036 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -34,7 +34,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock lights/blinkm lights/rgbled diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index fa02d95035..41a04e22da 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -31,7 +31,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver #irlock #lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 82fbc75c50..f16d940676 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -43,7 +43,6 @@ px4_add_board( #imu/invensense/icm20948 imu/invensense/mpu6000 #imu/invensense/mpu9250 - #imu/mpu6000 # legacy mpu6000 #iridiumsbd #irlock #lights/blinkm diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 7d99cd65f3..ded2a74ae0 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -26,7 +26,7 @@ px4_add_board( gps imu/l3gd20 imu/lsm303d - imu/mpu6000 + imu/invensense/mpu6000 #imu/invensense/mpu9250 irlock lights/rgbled diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index db5923eb3a..9018ef1252 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -34,7 +34,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index e74ab83236..eb12c7869b 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -35,7 +35,6 @@ px4_add_board( #imu/bosch/bmi055 #imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver #irlock #lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index c2dcd651ab..ab8e6e835a 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -34,7 +34,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 8c333bcb4b..8e961a93c6 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -30,7 +30,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index ba85501855..00b8f669cc 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -31,7 +31,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index c63b47973c..7c223aa82d 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -33,7 +33,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock #lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 331e5fc07c..8cd807c3a1 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -28,7 +28,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver lights/rgbled lights/rgbled_ncp5623c lights/rgbled_pwm diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 37c253aead..ee49a2e12c 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -33,7 +33,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 387c142a51..8b8a36fccf 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -35,7 +35,6 @@ px4_add_board( #imu/bosch/bmi055 #imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver #irlock #lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/uavcanv0periph.cmake b/boards/px4/fmu-v5/uavcanv0periph.cmake index 2459f85ac6..ad7a51ef3e 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.cmake +++ b/boards/px4/fmu-v5/uavcanv0periph.cmake @@ -36,7 +36,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver #irlock #lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index 765326d8f1..bf9f05c586 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -34,7 +34,6 @@ px4_add_board( imu/bosch/bmi055 imu/invensense/icm20602 imu/invensense/icm20689 - #imu/mpu6000 # legacy icm20602/icm20689 driver irlock lights/blinkm lights/rgbled diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index e82de24a40..248546beda 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -28,7 +28,7 @@ px4_add_board( #imu/adis16477 #imu/adis16497 #imu/bmi088 - imu/mpu6000 + imu/invensense/mpu6000 imu/invensense/icm20602 #imu/mpu9250 #irlock diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index 459b27b8fa..38c3341152 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -105,8 +105,8 @@ # DRIVERS # barometer/ms5611 # gps -# imu/bmi055 -# imu/mpu6000 +# imu/bosch/bmi055 +# imu/invensense/mpu6000 # magnetometer/isentek/ist8310 # pwm_out # px4io diff --git a/src/drivers/imu/mpu6000/CMakeLists.txt b/src/drivers/imu/mpu6000/CMakeLists.txt deleted file mode 100644 index ad8aa89cf1..0000000000 --- a/src/drivers/imu/mpu6000/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE drivers__mpu6000 - MAIN mpu6000 - SRCS - MPU6000.cpp - MPU6000_I2C.cpp - mpu6000_main.cpp - MPU6000_SPI.cpp - DEPENDS - drivers_accelerometer - drivers_gyroscope - ) diff --git a/src/drivers/imu/mpu6000/MPU6000.cpp b/src/drivers/imu/mpu6000/MPU6000.cpp deleted file mode 100644 index 3545ae522a..0000000000 --- a/src/drivers/imu/mpu6000/MPU6000.cpp +++ /dev/null @@ -1,903 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 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 "MPU6000.hpp" - -/* - list of registers that will be checked in check_registers(). Note - that MPUREG_PRODUCT_ID must be first in the list. - */ -constexpr uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; - -MPU6000::MPU6000(device::Device *interface, enum Rotation rotation, int device_type, I2CSPIBusOption bus_option, - int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, 0, device_type), - _interface(interface), - _device_type(device_type), - _px4_accel(_interface->get_device_id(), rotation), - _px4_gyro(_interface->get_device_id(), rotation), - _sample_perf(perf_alloc(PC_ELAPSED, "mpu6k_read")), - _bad_transfers(perf_alloc(PC_COUNT, "mpu6k_bad_trans")), - _bad_registers(perf_alloc(PC_COUNT, "mpu6k_bad_reg")), - _reset_retries(perf_alloc(PC_COUNT, "mpu6k_reset")), - _duplicates(perf_alloc(PC_COUNT, "mpu6k_duplicates")) -{ - switch (_device_type) { - default: - case MPU_DEVICE_TYPE_MPU6000: - _interface->set_device_type(DRV_IMU_DEVTYPE_MPU6000); - _px4_accel.set_device_type(DRV_IMU_DEVTYPE_MPU6000); - _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_MPU6000); - break; - - case MPU_DEVICE_TYPE_ICM20602: - _interface->set_device_type(DRV_IMU_DEVTYPE_ICM20602); - _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20602); - _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20602); - break; - - case MPU_DEVICE_TYPE_ICM20608: - _interface->set_device_type(DRV_IMU_DEVTYPE_ICM20608G); - _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20608G); - _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20608G); - break; - - case MPU_DEVICE_TYPE_ICM20689: - _interface->set_device_type(DRV_IMU_DEVTYPE_ICM20689); - _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM20689); - _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM20689); - break; - } -} - -MPU6000::~MPU6000() -{ - perf_free(_sample_perf); - perf_free(_bad_transfers); - perf_free(_bad_registers); - perf_free(_reset_retries); - perf_free(_duplicates); -} - -int -MPU6000::init() -{ - /* probe again to get our settings that are based on the device type */ - int ret = probe(); - - /* if probe failed, bail now */ - if (ret != OK) { - PX4_DEBUG("probe init failed"); - return ret; - } - - if (reset() != OK) { - return ret; - } - - return ret; -} - -int MPU6000::reset() -{ - // if the mpu6000 is initialized after the l3gd20 and lsm303d - // then if we don't do an irqsave/irqrestore here the mpu6000 - // frequently comes up in a bad state where all transfers - // come as zero - uint8_t tries = 5; - irqstate_t state; - - while (--tries != 0) { - state = px4_enter_critical_section(); - - // Hold off sampling for 60 ms - _reset_wait = hrt_absolute_time() + 60000; - - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); - - // Wake up device and select GyroZ clock. Note that the - // MPU6000 starts up in sleep mode, and it can take some time - // for it to come out of sleep - write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); - - // Enable I2C bus or Disable I2C bus (recommended on data sheet) - const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); - write_checked_reg(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS); - - px4_leave_critical_section(state); - - if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { - break; - } - - perf_count(_reset_retries); - px4_usleep(2000); - } - - // Hold off sampling for 30 ms - - state = px4_enter_critical_section(); - _reset_wait = hrt_absolute_time() + 30000; - px4_leave_critical_section(state); - - if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { - return -EIO; - } - - px4_usleep(1000); - - // SAMPLE RATE - _set_sample_rate(1000); - px4_usleep(1000); - - _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); - - if (is_icm_device()) { - _set_icm_acc_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); - } - - px4_usleep(1000); - // Gyro scale 2000 deg/s () - write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); - px4_usleep(1000); - - // correct gyro scale factors - // scale to rad/s in SI units - // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s - // scaling factor: - // 1/(2^15)*(2000/180)*PI - _px4_gyro.set_scale(0.0174532f / 16.4f);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); - - set_accel_range(MPU6000_ACCEL_DEFAULT_RANGE_G); - - px4_usleep(1000); - - // INT CFG => Interrupt on Data Ready - write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready - px4_usleep(1000); - write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read - px4_usleep(1000); - - if (is_icm_device()) { - write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE); - } - - // Oscillator set - // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); - px4_usleep(1000); - return OK; -} - -int -MPU6000::probe() -{ - uint8_t whoami = read_reg(MPUREG_WHOAMI); - uint8_t expected = 0; - bool unknown_product_id = true; - - switch (_device_type) { - - default: - case MPU_DEVICE_TYPE_MPU6000: - expected = MPU_WHOAMI_6000; - break; - - case MPU_DEVICE_TYPE_ICM20602: - expected = ICM_WHOAMI_20602; - break; - - case MPU_DEVICE_TYPE_ICM20608: - expected = ICM_WHOAMI_20608; - break; - - case MPU_DEVICE_TYPE_ICM20689: - expected = ICM_WHOAMI_20689; - break; - } - - if (whoami != expected) { - PX4_DEBUG("unexpected WHOAMI 0x%02x", whoami); - return -EIO; - } - - /* look for a product ID we recognize */ - _product = read_reg(MPUREG_PRODUCT_ID); - - // verify product revision - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - case MPU6000ES_REV_D6: - case MPU6000ES_REV_D7: - case MPU6000ES_REV_D8: - case MPU6000_REV_D6: - case MPU6000_REV_D7: - case MPU6000_REV_D8: - case MPU6000_REV_D9: - case MPU6000_REV_D10: - case ICM20608_REV_FF: - case ICM20689_REV_FE: - case ICM20689_REV_03: - case ICM20689_REV_04: - case ICM20602_REV_01: - case ICM20602_REV_02: - case MPU6050_REV_D8: - unknown_product_id = false; - } - - _checked_values[MPU6000_CHECKED_PRODUCT_ID_INDEX] = _product; - - PX4_DEBUG("ID 0x%02x", _product); - - if (unknown_product_id) { - - PX4_WARN("unexpected ID 0x%02x %s", _product, is_icm_device() ? "accepted" : "exiting!"); - - if (is_mpu_device()) { - return -EIO; - } - } - - return OK; -} - -/* - set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro -*/ -void -MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) -{ - if (desired_sample_rate_hz == 0) { - desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; - } - - uint8_t div = 1000 / desired_sample_rate_hz; - - if (div > 200) { - div = 200; - } - - if (div < 1) { - div = 1; - } - - write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); - _sample_rate = 1000 / div; -} - -/* - set the DLPF filter frequency. This affects both accel and gyro. - */ -void -MPU6000::_set_dlpf_filter(uint16_t frequency_hz) -{ - uint8_t filter = MPU_GYRO_DLPF_CFG_2100HZ_NOLPF; - - /* - choose next highest filter frequency available - */ - if (frequency_hz == 0) { - filter = MPU_GYRO_DLPF_CFG_2100HZ_NOLPF; - - } else if (frequency_hz <= 5) { - filter = MPU_GYRO_DLPF_CFG_5HZ; - - } else if (frequency_hz <= 10) { - filter = MPU_GYRO_DLPF_CFG_10HZ; - - } else if (frequency_hz <= 20) { - filter = MPU_GYRO_DLPF_CFG_20HZ; - - } else if (frequency_hz <= 42) { - filter = MPU_GYRO_DLPF_CFG_42HZ; - - } else if (frequency_hz <= 98) { - filter = MPU_GYRO_DLPF_CFG_98HZ; - - } else if (frequency_hz <= 188) { - filter = MPU_GYRO_DLPF_CFG_188HZ; - - } else if (frequency_hz <= 256) { - filter = MPU_GYRO_DLPF_CFG_256HZ_NOLPF2; - } - - write_checked_reg(MPUREG_CONFIG, filter); -} - -void -MPU6000::_set_icm_acc_dlpf_filter(uint16_t frequency_hz) -{ - uint8_t filter = ICM_ACC_DLPF_CFG_1046HZ_NOLPF; - - /* - choose next highest filter frequency available - */ - if (frequency_hz == 0) { - filter = ICM_ACC_DLPF_CFG_1046HZ_NOLPF; - - } else if (frequency_hz <= 5) { - filter = ICM_ACC_DLPF_CFG_5HZ; - - } else if (frequency_hz <= 10) { - filter = ICM_ACC_DLPF_CFG_10HZ; - - } else if (frequency_hz <= 21) { - filter = ICM_ACC_DLPF_CFG_21HZ; - - } else if (frequency_hz <= 44) { - filter = ICM_ACC_DLPF_CFG_44HZ; - - } else if (frequency_hz <= 99) { - filter = ICM_ACC_DLPF_CFG_99HZ; - - } else if (frequency_hz <= 218) { - filter = ICM_ACC_DLPF_CFG_218HZ; - - } else if (frequency_hz <= 420) { - filter = ICM_ACC_DLPF_CFG_420HZ; - } - - write_checked_reg(ICMREG_ACCEL_CONFIG2, filter); -} - -#ifndef CONSTRAINED_FLASH -/* - perform a self-test comparison to factory trim values. This takes - about 200ms and will return OK if the current values are within 14% - of the expected values (as per datasheet) - */ -void -MPU6000::factory_self_test() -{ - _in_factory_test = true; - uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); - uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); - const uint16_t repeats = 100; - int ret = OK; - - // gyro self test has to be done at 250DPS - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - - struct MPUReport mpu_report {}; - float accel_baseline[3] {}; - float gyro_baseline[3] {}; - float accel[3] {}; - float gyro[3] {}; - float accel_ftrim[3] {}; - float gyro_ftrim[3] {}; - - // get baseline values without self-test enabled - for (uint8_t i = 0; i < repeats; i++) { - up_udelay(1000); - _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report, - sizeof(mpu_report)); - - accel_baseline[0] += int16_t_from_bytes(mpu_report.accel_x); - accel_baseline[1] += int16_t_from_bytes(mpu_report.accel_y); - accel_baseline[2] += int16_t_from_bytes(mpu_report.accel_z); - gyro_baseline[0] += int16_t_from_bytes(mpu_report.gyro_x); - gyro_baseline[1] += int16_t_from_bytes(mpu_report.gyro_y); - gyro_baseline[2] += int16_t_from_bytes(mpu_report.gyro_z); - } - -#if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | - BITS_GYRO_ST_Z); - - // accel 8g, self-test enabled all axes - write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config | 0xE0); -#endif - - up_udelay(20000); - - // get values with self-test enabled - for (uint8_t i = 0; i < repeats; i++) { - up_udelay(1000); - _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), (uint8_t *)&mpu_report, - sizeof(mpu_report)); - - accel[0] += int16_t_from_bytes(mpu_report.accel_x); - accel[1] += int16_t_from_bytes(mpu_report.accel_y); - accel[2] += int16_t_from_bytes(mpu_report.accel_z); - gyro[0] += int16_t_from_bytes(mpu_report.gyro_x); - gyro[1] += int16_t_from_bytes(mpu_report.gyro_y); - gyro[2] += int16_t_from_bytes(mpu_report.gyro_z); - } - - for (uint8_t i = 0; i < 3; i++) { - accel_baseline[i] /= repeats; - gyro_baseline[i] /= repeats; - accel[i] /= repeats; - gyro[i] /= repeats; - } - - // extract factory trim values - uint8_t trims[4]; - trims[0] = read_reg(MPUREG_TRIM1); - trims[1] = read_reg(MPUREG_TRIM2); - trims[2] = read_reg(MPUREG_TRIM3); - trims[3] = read_reg(MPUREG_TRIM4); - uint8_t atrim[3]; - uint8_t gtrim[3]; - - atrim[0] = ((trims[0] >> 3) & 0x1C) | ((trims[3] >> 4) & 0x03); - atrim[1] = ((trims[1] >> 3) & 0x1C) | ((trims[3] >> 2) & 0x03); - atrim[2] = ((trims[2] >> 3) & 0x1C) | ((trims[3] >> 0) & 0x03); - gtrim[0] = trims[0] & 0x1F; - gtrim[1] = trims[1] & 0x1F; - gtrim[2] = trims[2] & 0x1F; - - // convert factory trims to right units - for (uint8_t i = 0; i < 3; i++) { - accel_ftrim[i] = 4096 * 0.34f * powf(0.92f / 0.34f, (atrim[i] - 1) / 30.0f); - gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i] - 1); - } - - // Y gyro trim is negative - gyro_ftrim[1] *= -1; - - for (uint8_t i = 0; i < 3; i++) { - float diff = accel[i] - accel_baseline[i]; - float err = 100 * (diff - accel_ftrim[i]) / accel_ftrim[i]; - ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", - (unsigned)i, - (int)(1000 * accel_baseline[i]), - (int)(1000 * accel[i]), - (int)(1000 * diff), - (int)(1000 * accel_ftrim[i]), - (int)err); - - if (fabsf(err) > 14) { - ::printf("FAIL\n"); - ret = -EIO; - } - } - - for (uint8_t i = 0; i < 3; i++) { - float diff = gyro[i] - gyro_baseline[i]; - float err = 100 * (diff - gyro_ftrim[i]) / gyro_ftrim[i]; - ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", - (unsigned)i, - (int)(1000 * gyro_baseline[i]), - (int)(1000 * gyro[i]), - (int)(1000 * (gyro[i] - gyro_baseline[i])), - (int)(1000 * gyro_ftrim[i]), - (int)err); - - if (fabsf(err) > 14) { - ::printf("FAIL\n"); - ret = -EIO; - } - } - - write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); - write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); - - _in_factory_test = false; - - if (ret == OK) { - ::printf("PASSED\n"); - } - -} -#endif - -/* - deliberately trigger an error in the sensor to trigger recovery - */ -void -MPU6000::test_error() -{ - _in_factory_test = true; - // deliberately trigger an error. This was noticed during - // development as a handy way to test the reset logic - uint8_t data[sizeof(MPUReport)] {}; - _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data)); - PX4_WARN("error triggered"); - print_registers(); - _in_factory_test = false; -} - -uint8_t -MPU6000::read_reg(unsigned reg, uint32_t speed) -{ - uint8_t buf{}; - _interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1); - return buf; -} - -uint16_t -MPU6000::read_reg16(unsigned reg) -{ - uint8_t buf[2] {}; - - // general register transfer at low clock speed - _interface->read(MPU6000_LOW_SPEED_OP(reg), &buf, arraySize(buf)); - return (uint16_t)(buf[0] << 8) | buf[1]; -} - -int -MPU6000::write_reg(unsigned reg, uint8_t value) -{ - // general register transfer at low clock speed - return _interface->write(MPU6000_LOW_SPEED_OP(reg), &value, 1); -} - -void -MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -void -MPU6000::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < MPU6000_NUM_CHECKED_REGISTERS; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - } - } -} - -int -MPU6000::set_accel_range(unsigned max_g_in) -{ - // workaround for bugged versions of MPU6k (rev C) - if (is_mpu_device()) { - switch (_product) { - case MPU6000ES_REV_C4: - case MPU6000ES_REV_C5: - case MPU6000_REV_C4: - case MPU6000_REV_C5: - write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.0f); - return OK; - } - } - - uint8_t afs_sel; - float lsb_per_g; - //float max_accel_g; - - if (max_g_in > 8) { // 16g - AFS_SEL = 3 - afs_sel = 3; - lsb_per_g = 2048; - //max_accel_g = 16; - - } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 - afs_sel = 2; - lsb_per_g = 4096; - //max_accel_g = 8; - - } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 - afs_sel = 1; - lsb_per_g = 8192; - //max_accel_g = 4; - - } else { // 2g - AFS_SEL = 0 - afs_sel = 0; - lsb_per_g = 16384; - //max_accel_g = 2; - } - - write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); - - _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g); - - return OK; -} - -void -MPU6000::start() -{ - /* make sure we are stopped first */ - uint32_t last_call_interval = _call_interval; - stop(); - _call_interval = last_call_interval; - - ScheduleOnInterval(_call_interval - MPU6000_TIMER_REDUCTION, 1000); -} - -void -MPU6000::stop() -{ - ScheduleClear(); - - /* reset internal states */ - memset(_last_accel, 0, sizeof(_last_accel)); -} - -void -MPU6000::check_registers(void) -{ - /* - we read the register at full speed, even though it isn't - listed as a high speed register. The low speed requirement - for some registers seems to be a propgation delay - requirement for changing sensor configuration, which should - not apply to reading a single register. It is also a better - test of SPI bus health to read at the same speed as we read - the data registers. - */ - uint8_t v; - - // the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented) - if ((_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device())) { - _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; - } - - if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != - _checked_values[_checked_next]) { - /* - if we get the wrong value then we know the SPI bus - or sensor is very sick. We set _register_wait to 20 - and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. - */ - perf_count(_bad_registers); - - /* - try to fix the bad register value. We only try to - fix one per loop to prevent a bad sensor hogging the - bus. - */ - if (_register_wait == 0 || _checked_next == MPU6000_CHECKED_PRODUCT_ID_INDEX) { - // if the product_id is wrong then reset the - // sensor completely - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - // after doing a reset we need to wait a long - // time before we do any other register writes - // or we will end up with the mpu6000 in a - // bizarre state where it has all correct - // register values but large offsets on the - // accel axes - _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; - - } else { - write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - // waiting 3ms between register writes seems - // to raise the chance of the sensor - // recovering considerably - _reset_wait = hrt_absolute_time() + 3000; - } - - _register_wait = 20; - } - - _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; -} - -void MPU6000::RunImpl() -{ - if (_in_factory_test) { - // don't publish any data while in factory test mode - return; - } - - if (hrt_absolute_time() < _reset_wait) { - // we're waiting for a reset to complete - return; - } - - struct MPUReport mpu_report; - - struct Report { - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; - int16_t temp; - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - } report; - - /* start measuring */ - perf_begin(_sample_perf); - - /* - * Fetch the full set of measurements from the MPU6000 in one pass. - */ - - // sensor transfer at high clock speed - - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED), - (uint8_t *)&mpu_report, sizeof(mpu_report))) { - - perf_end(_sample_perf); - return; - } - - check_registers(); - - /* - see if this is duplicate accelerometer data. Note that we - can't use the data ready interrupt status bit in the status - register as that also goes high on new gyro data, and when - we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being - sampled at 8kHz, so we would incorrectly think we have new - data when we are in fact getting duplicate accelerometer data. - */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { - // it isn't new data - wait for next timer - perf_end(_sample_perf); - perf_count(_duplicates); - _got_duplicate = true; - return; - } - - perf_end(_sample_perf); - - memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); - _got_duplicate = false; - - /* - * Convert from big to little endian - */ - - report.accel_x = int16_t_from_bytes(mpu_report.accel_x); - report.accel_y = int16_t_from_bytes(mpu_report.accel_y); - report.accel_z = int16_t_from_bytes(mpu_report.accel_z); - - report.temp = int16_t_from_bytes(mpu_report.temp); - - report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); - report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); - report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); - - if (report.accel_x == 0 && - report.accel_y == 0 && - report.accel_z == 0 && - report.temp == 0 && - report.gyro_x == 0 && - report.gyro_y == 0 && - report.gyro_z == 0) { - - // all zero data - probably a SPI bus error - perf_count(_bad_transfers); - - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, - return; - } - - if (_register_wait != 0) { - // we are waiting for some good transfers before using - // the sensor again, don't return any data yet - _register_wait--; - return; - } - - - /* - * Swap axes and negate y - */ - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - - // 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 - // whether it has had failures - - const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); - _px4_accel.set_error_count(error_count); - _px4_gyro.set_error_count(error_count); - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - float temperature = 0.0f; - - if (is_icm_device()) { // if it is an ICM20608 - temperature = (report.temp / 326.8f + 25.0f); - - } else { // If it is an MPU6000 - temperature = (report.temp / 340.0f + 35.0f); - } - - _px4_accel.set_temperature(temperature); - _px4_gyro.set_temperature(temperature); - - _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); - _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); -} - -void -MPU6000::print_status() -{ - I2CSPIDriverBase::print_status(); - PX4_INFO("Device type: %i", _device_type); - perf_print_counter(_sample_perf); - perf_print_counter(_bad_transfers); - perf_print_counter(_bad_registers); - perf_print_counter(_reset_retries); - perf_print_counter(_duplicates); - -} - -void -MPU6000::print_registers() -{ - PX4_INFO("registers"); - - for (uint8_t reg = MPUREG_PRODUCT_ID; reg <= 108; reg++) { - uint8_t v = read_reg(reg); - printf("%02x:%02x ", (unsigned)reg, (unsigned)v); - - if ((reg - (MPUREG_PRODUCT_ID - 1)) % 13 == 0) { - printf("\n"); - } - } - - printf("\n"); -} diff --git a/src/drivers/imu/mpu6000/MPU6000.hpp b/src/drivers/imu/mpu6000/MPU6000.hpp deleted file mode 100644 index fef7962745..0000000000 --- a/src/drivers/imu/mpu6000/MPU6000.hpp +++ /dev/null @@ -1,455 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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 mpu6000.cpp - * - * Driver for the Invensense MPU6000, MPU6050, ICM20608, and ICM20602 connected via - * SPI or I2C. - * - * When the device is on the SPI bus the hrt is used to provide thread of - * execution to the driver. - * - * When the device is on the I2C bus a work queue is used provide thread of - * execution to the driver. - * - * The I2C code is only included in the build if USE_I2C is defined by the - * existance of any of PX4_I2C_MPU6050_ADDR, PX4_I2C_MPU6000_ADDR - * PX4_I2C_ICM_20608_G_ADDR in the board_config.h file. - * - * The command line option -T 6000|20608|20602 (default 6000) selects between - * MPU60x0, ICM20608G, or ICM20602G; - * - * @author Andrew Tridgell - * @author Pat Hickey - * @author David Sidrane - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -/* - we set the timer interrupt to run a bit faster than the desired - sample rate and then throw away duplicates by comparing - accelerometer values. This time reduction is enough to cope with - worst case timing jitter due to other timers - - I2C bus is running at 100 kHz Transaction time is 2.163ms - I2C bus is running at 400 kHz (304 kHz actual) Transaction time - is 583 us - - */ - -#pragma once - -#if defined(PX4_I2C_MPU6050_ADDR) || \ - defined(PX4_I2C_MPU6000_ADDR) || \ - defined(PX4_I2C_ICM_20608_G_ADDR) -# define USE_I2C -#endif - -enum MPU_DEVICE_TYPE { - MPU_DEVICE_TYPE_MPU6000 = 6000, - MPU_DEVICE_TYPE_ICM20602 = 20602, - MPU_DEVICE_TYPE_ICM20608 = 20608, - MPU_DEVICE_TYPE_ICM20689 = 20689 -}; - -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -// MPU 6000 registers -#define MPUREG_WHOAMI 0x75 -#define MPUREG_SMPLRT_DIV 0x19 -#define MPUREG_CONFIG 0x1A -#define MPUREG_GYRO_CONFIG 0x1B -#define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 -#define MPUREG_INT_STATUS 0x3A -#define MPUREG_ACCEL_XOUT_H 0x3B -#define MPUREG_ACCEL_XOUT_L 0x3C -#define MPUREG_ACCEL_YOUT_H 0x3D -#define MPUREG_ACCEL_YOUT_L 0x3E -#define MPUREG_ACCEL_ZOUT_H 0x3F -#define MPUREG_ACCEL_ZOUT_L 0x40 -#define MPUREG_TEMP_OUT_H 0x41 -#define MPUREG_TEMP_OUT_L 0x42 -#define MPUREG_GYRO_XOUT_H 0x43 -#define MPUREG_GYRO_XOUT_L 0x44 -#define MPUREG_GYRO_YOUT_H 0x45 -#define MPUREG_GYRO_YOUT_L 0x46 -#define MPUREG_GYRO_ZOUT_H 0x47 -#define MPUREG_GYRO_ZOUT_L 0x48 -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C -#define MPUREG_FIFO_COUNTH 0x72 -#define MPUREG_FIFO_COUNTL 0x73 -#define MPUREG_FIFO_R_W 0x74 -#define MPUREG_PRODUCT_ID 0x0C -#define MPUREG_TRIM1 0x0D -#define MPUREG_TRIM2 0x0E -#define MPUREG_TRIM3 0x0F -#define MPUREG_TRIM4 0x10 -#define MPU_GYRO_DLPF_CFG_256HZ_NOLPF2 0x00 // delay: 0.98ms -#define MPU_GYRO_DLPF_CFG_188HZ 0x01 // delay: 1.9ms -#define MPU_GYRO_DLPF_CFG_98HZ 0x02 // delay: 2.8ms -#define MPU_GYRO_DLPF_CFG_42HZ 0x03 // delay: 4.8ms -#define MPU_GYRO_DLPF_CFG_20HZ 0x04 // delay: 8.3ms -#define MPU_GYRO_DLPF_CFG_10HZ 0x05 // delay: 13.4ms -#define MPU_GYRO_DLPF_CFG_5HZ 0x06 // delay: 18.6ms -#define MPU_GYRO_DLPF_CFG_2100HZ_NOLPF 0x07 -#define MPU_DLPF_CFG_MASK 0x07 - -// Configuration bits MPU 3000 and MPU 6000 (not revised)? -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define BITS_CLKSEL 0x07 -#define MPU_CLK_SEL_PLLGYROX 0x01 -#define MPU_CLK_SEL_PLLGYROZ 0x03 -#define MPU_EXT_SYNC_GYROX 0x02 -#define BITS_GYRO_ST_X 0x80 -#define BITS_GYRO_ST_Y 0x40 -#define BITS_GYRO_ST_Z 0x20 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_RAW_RDY_EN 0x01 -#define BIT_I2C_IF_DIS 0x10 -#define BIT_INT_STATUS_DATA 0x01 - -#define MPU_WHOAMI_6000 0x68 -#define ICM_WHOAMI_20602 0x12 -#define ICM_WHOAMI_20608 0xaf -#define ICM_WHOAMI_20689 0x98 - -// ICM2608 specific registers - -#define ICMREG_ACCEL_CONFIG2 0x1D -#define ICM_ACC_DLPF_CFG_1046HZ_NOLPF 0x00 -#define ICM_ACC_DLPF_CFG_218HZ 0x01 -#define ICM_ACC_DLPF_CFG_99HZ 0x02 -#define ICM_ACC_DLPF_CFG_44HZ 0x03 -#define ICM_ACC_DLPF_CFG_21HZ 0x04 -#define ICM_ACC_DLPF_CFG_10HZ 0x05 -#define ICM_ACC_DLPF_CFG_5HZ 0x06 -#define ICM_ACC_DLPF_CFG_420HZ 0x07 -/* this is an undocumented register which - if set incorrectly results in getting a 2.7m/s/s offset - on the Y axis of the accelerometer -*/ -#define MPUREG_ICM_UNDOC1 0x11 -#define MPUREG_ICM_UNDOC1_VALUE 0xc9 - -// Product ID Description for ICM20602 -// Read From device - -#define ICM20602_REV_01 1 -#define ICM20602_REV_02 2 - -// Product ID Description for ICM20608 - -#define ICM20608_REV_FF 0xff // In the past, was thought to be not returning a value. But seem repeatable. - -// Product ID Description for ICM20689 - -#define ICM20689_REV_FE 0xfe -#define ICM20689_REV_03 0x03 -#define ICM20689_REV_04 0x04 - -// Product ID Description for MPU6000 -// high 4 bits low 4 bits -// Product Name Product Revision -#define MPU6000ES_REV_C4 0x14 -#define MPU6000ES_REV_C5 0x15 -#define MPU6000ES_REV_D6 0x16 -#define MPU6000ES_REV_D7 0x17 -#define MPU6000ES_REV_D8 0x18 -#define MPU6000_REV_C4 0x54 -#define MPU6000_REV_C5 0x55 -#define MPU6000_REV_D6 0x56 -#define MPU6000_REV_D7 0x57 -#define MPU6000_REV_D8 0x58 -#define MPU6000_REV_D9 0x59 -#define MPU6000_REV_D10 0x5A -#define MPU6050_REV_D8 0x28 // TODO:Need verification - -#define MPU6000_ACCEL_DEFAULT_RANGE_G 16 - -#define MPU6000_GYRO_DEFAULT_RANGE_G 8 -#define MPU6000_GYRO_DEFAULT_RATE 1000 - - -#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 98 - -#pragma pack(push, 1) -/** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ -struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; -}; -#pragma pack(pop) - -#define MPU_MAX_READ_BUFFER_SIZE (sizeof(MPUReport) + 1) -#define MPU_MAX_WRITE_BUFFER_SIZE (2) -/* - The MPU6000 can only handle high bus speeds on the sensor and - interrupt status registers. All other registers have a maximum 1MHz - Communication with all registers of the device is performed using either - I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications, - the sensor and interrupt registers may be read using SPI at 20MHz - */ -#define MPU6000_LOW_BUS_SPEED 0 -#define MPU6000_HIGH_BUS_SPEED 0x8000 -# define MPU6000_IS_HIGH_SPEED(r) ((r) & MPU6000_HIGH_BUS_SPEED) -# define MPU6000_REG(r) ((r) &~MPU6000_HIGH_BUS_SPEED) -# define MPU6000_SET_SPEED(r, s) ((r)|(s)) -# define MPU6000_HIGH_SPEED_OP(r) MPU6000_SET_SPEED((r), MPU6000_HIGH_BUS_SPEED) -# define MPU6000_LOW_SPEED_OP(r) MPU6000_REG((r)) - -/* interface factories */ -extern device::Device *MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, - int bus_frequency, spi_mode_e spi_mode); -extern device::Device *MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, - int bus_frequency); -extern int MPU6000_probe(device::Device *dev, int device_type); - - -#define MPU6000_TIMER_REDUCTION 200 - - -class MPU6000 : public I2CSPIDriver -{ -public: - MPU6000(device::Device *interface, enum Rotation rotation, int device_type, I2CSPIBusOption bus_option, int bus); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - virtual ~MPU6000(); - - int init(); - - void print_registers(); - -#ifndef CONSTRAINED_FLASH - /** - * Test behaviour against factory offsets - */ - void factory_self_test(); -#endif - - // deliberately cause a sensor error - void test_error(); - - /** - * Start automatic measurement. - */ - void start(); - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - void RunImpl(); - -protected: - device::Device *_interface; - - int probe(); - - void print_status() override; - - void custom_method(const BusCLIArguments &cli) override; - -private: - - int _device_type; - uint8_t _product{0}; /** product code */ - - unsigned _call_interval{1000}; - - PX4Accelerometer _px4_accel; - PX4Gyroscope _px4_gyro; - - unsigned _sample_rate{1000}; - - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _bad_registers; - perf_counter_t _reset_retries; - perf_counter_t _duplicates; - - uint8_t _register_wait{0}; - uint64_t _reset_wait{0}; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset - static constexpr int MPU6000_CHECKED_PRODUCT_ID_INDEX = 0; - static constexpr int MPU6000_NUM_CHECKED_REGISTERS = 10; - - static constexpr uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS] { - MPUREG_PRODUCT_ID, - MPUREG_PWR_MGMT_1, - MPUREG_USER_CTRL, - MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, - MPUREG_GYRO_CONFIG, - MPUREG_ACCEL_CONFIG, - MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG, - MPUREG_ICM_UNDOC1 - }; - - uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next{0}; - - // use this to avoid processing measurements when in factory - // self test - volatile bool _in_factory_test{false}; - - // keep last accel reading for duplicate detection - uint16_t _last_accel[3] {}; - bool _got_duplicate{false}; - - /** - * Stop automatic measurement. - */ - void stop(); - - /** - * is_icm_device - */ - bool is_icm_device() { return !is_mpu_device(); } - /** - * is_mpu_device - */ - bool is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; } - - /** - * Read a register from the MPU6000 - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED); - uint16_t read_reg16(unsigned reg); - - - /** - * Write a register in the MPU6000 - * - * @param reg The register to write. - * @param value The new value to write. - */ - int write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the MPU6000 - * - * 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 MPU6000, 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 MPU6000 measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_accel_range(unsigned max_g); - - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); - void _set_icm_acc_dlpf_filter(uint16_t frequency_hz); - - /* - set sample rate (approximate) - 1kHz to 5Hz - */ - void _set_sample_rate(unsigned 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 */ - MPU6000(const MPU6000 &); - MPU6000 operator=(const MPU6000 &); - -}; diff --git a/src/drivers/imu/mpu6000/MPU6000_I2C.cpp b/src/drivers/imu/mpu6000/MPU6000_I2C.cpp deleted file mode 100644 index ce8f24f5ca..0000000000 --- a/src/drivers/imu/mpu6000/MPU6000_I2C.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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 mpu6000_i2c.cpp - * - * I2C interface for MPU6000 /MPU6050 - */ - -#include - -#include "MPU6000.hpp" - -#ifdef PX4_I2C_MPU6050_ADDR - -device::Device *MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency); - -class MPU6000_I2C : public device::I2C -{ -public: - MPU6000_I2C(int bus, int device_type, int bus_frequency); - ~MPU6000_I2C() override = default; - - int read(unsigned address, void *data, unsigned count) override; - int write(unsigned address, void *data, unsigned count) override; - -protected: - int probe() override; - -private: - int _device_type; - -}; - - -device::Device * -MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency) -{ - return new MPU6000_I2C(bus, device_type, bus_frequency); -} - -MPU6000_I2C::MPU6000_I2C(int bus, int device_type, int bus_frequency) : - I2C(DRV_IMU_DEVTYPE_MPU6000, MODULE_NAME, bus, PX4_I2C_MPU6050_ADDR, bus_frequency) - _device_type(device_type) -{ -} - -int -MPU6000_I2C::write(unsigned reg_speed, void *data, unsigned count) -{ - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; - - if (sizeof(cmd) < (count + 1)) { - return -EIO; - } - - cmd[0] = MPU6000_REG(reg_speed); - cmd[1] = *(uint8_t *)data; - return transfer(&cmd[0], count + 1, nullptr, 0); -} - -int -MPU6000_I2C::read(unsigned reg_speed, void *data, unsigned count) -{ - /* We want to avoid copying the data of MPUReport: So if the caller - * supplies a buffer not MPUReport in size, it is assume to be a reg or - * reg 16 read - * Since MPUReport has a cmd at front, we must return the data - * after that. Foe anthing else we must return it - */ - uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status); - uint8_t cmd = MPU6000_REG(reg_speed); - int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count - offset); - return ret == OK ? count : ret; -} - -int -MPU6000_I2C::probe() -{ - uint8_t whoami = 0; - uint8_t expected = _device_type == MPU_DEVICE_TYPE_MPU6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608; - return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO; - -} -#else - -device::Device * -MPU6000_I2C_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency) -{ - return nullptr; -} - -#endif /* USE_I2C */ diff --git a/src/drivers/imu/mpu6000/MPU6000_SPI.cpp b/src/drivers/imu/mpu6000/MPU6000_SPI.cpp deleted file mode 100644 index 9de42078d2..0000000000 --- a/src/drivers/imu/mpu6000/MPU6000_SPI.cpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 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 mpu6000_spi.cpp - * - * Driver for the Invensense MPU6000 connected via SPI. - * - * @author Andrew Tridgell - * @author Pat Hickey - * @author David sidrane - */ - -#include - -#include "MPU6000.hpp" - -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - - -/* The MPU6000 can only handle high SPI bus speeds of 20Mhz on the sensor and -* interrupt status registers. All other registers have a maximum 1MHz -* SPI speed -* -* The ICM parts are not rated as high. -* -* The Actual Value will be rounded down by the spi driver. -* 168 Mhz CPU 180 Mhz CPU -* Selected ------------actual--------------- -* 20 Mhz 10.5 Mhz 11.250 Mhz -* 10 Mhz 5.250 Mhz 5.625 Mhz -* 8 Mhz 5.250 Mhz 5.625 Mhz -* 1 Mhz 0.703125 Mhz 0.65625 Mhz -* -*/ -#define MPU6000_LOW_SPI_BUS_SPEED 1000*1000 -#define MPU6000_HIGH_SPI_BUS_SPEED 20*1000*1000 -#define ICM20608_HIGH_SPI_BUS_SPEED 8*1000*1000 -#define ICM20689_HIGH_SPI_BUS_SPEED 8*1000*1000 -#define ICM20602_HIGH_SPI_BUS_SPEED 10*1000*1000 -#define UNKNOWN_HIGH_SPI_BUS_SPEED 8*1000*1000 // Use the minimum - - -device::Device *MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency, - spi_mode_e spi_mode); - - -class MPU6000_SPI : public device::SPI -{ -public: - MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode); - ~MPU6000_SPI() override = default; - - int read(unsigned address, void *data, unsigned count) override; - int write(unsigned address, void *data, unsigned count) override; - -protected: - int probe() override; - -private: - - int _device_type; - /* Helper to set the desired speed and isolate the register on return */ - - int _max_frequency; - void set_bus_frequency(unsigned ®_speed_reg_out); -}; - -device::Device * -MPU6000_SPI_interface(int bus, uint32_t devid, int device_type, bool external_bus, int bus_frequency, - spi_mode_e spi_mode) -{ - return new MPU6000_SPI(bus, devid, device_type, bus_frequency, spi_mode); -} - -MPU6000_SPI::MPU6000_SPI(int bus, uint32_t device, int device_type, int bus_frequency, spi_mode_e spi_mode) : - SPI(DRV_IMU_DEVTYPE_MPU6000, MODULE_NAME, bus, device, spi_mode, bus_frequency), - _device_type(device_type) -{ -} - -void -MPU6000_SPI::set_bus_frequency(unsigned ®_speed) -{ - /* Set the desired speed */ - set_frequency(MPU6000_IS_HIGH_SPEED(reg_speed) ? _max_frequency : MPU6000_LOW_SPI_BUS_SPEED); - - /* Isolate the register on return */ - reg_speed = MPU6000_REG(reg_speed); -} - -int -MPU6000_SPI::write(unsigned reg_speed, void *data, unsigned count) -{ - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; - - if (sizeof(cmd) < (count + 1)) { - return -EIO; - } - - /* Set the desired speed and isolate the register */ - set_bus_frequency(reg_speed); - - cmd[0] = reg_speed | DIR_WRITE; - cmd[1] = *(uint8_t *)data; - - return transfer(&cmd[0], &cmd[0], count + 1); -} - -int -MPU6000_SPI::read(unsigned reg_speed, void *data, unsigned count) -{ - /* We want to avoid copying the data of MPUReport: So if the caller - * supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read - * and we need to provied the buffer large enough for the callers data - * and our command. - */ - uint8_t cmd[3] = {0, 0, 0}; - - uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ; - - if (count < sizeof(MPUReport)) { - /* add command */ - count++; - } - - set_bus_frequency(reg_speed); - - /* Set command */ - pbuff[0] = reg_speed | DIR_READ ; - - /* Transfer the command and get the data */ - int ret = transfer(pbuff, pbuff, count); - - if (ret == OK && pbuff == &cmd[0]) { - - /* Adjust the count back */ - count--; - - /* Return the data */ - memcpy(data, &cmd[1], count); - - } - - return ret == OK ? count : ret; -} - -int -MPU6000_SPI::probe() -{ - uint8_t whoami = 0; - uint8_t expected = MPU_WHOAMI_6000; - _max_frequency = UNKNOWN_HIGH_SPI_BUS_SPEED; - - switch (_device_type) { - - default: - case MPU_DEVICE_TYPE_MPU6000: - _max_frequency = MPU6000_HIGH_SPI_BUS_SPEED; - break; - - case MPU_DEVICE_TYPE_ICM20602: - expected = ICM_WHOAMI_20602; - _max_frequency = ICM20602_HIGH_SPI_BUS_SPEED; - break; - - case MPU_DEVICE_TYPE_ICM20608: - expected = ICM_WHOAMI_20608; - _max_frequency = ICM20608_HIGH_SPI_BUS_SPEED; - break; - - case MPU_DEVICE_TYPE_ICM20689: - expected = ICM_WHOAMI_20689; - _max_frequency = ICM20689_HIGH_SPI_BUS_SPEED; - break; - } - - return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO; -} diff --git a/src/drivers/imu/mpu6000/mpu6000_main.cpp b/src/drivers/imu/mpu6000/mpu6000_main.cpp deleted file mode 100644 index adde91207f..0000000000 --- a/src/drivers/imu/mpu6000/mpu6000_main.cpp +++ /dev/null @@ -1,213 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 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 "MPU6000.hpp" -#include -#include - -void -MPU6000::print_usage() -{ - PRINT_MODULE_USAGE_NAME("mpu6000", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("imu"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); - PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - - PRINT_MODULE_USAGE_PARAM_STRING('T', "6000", "6000|20608|20602|20689", "Device type", true); - - PRINT_MODULE_USAGE_COMMAND("reset"); - PRINT_MODULE_USAGE_COMMAND("regdump"); -#ifndef CONSTRAINED_FLASH - PRINT_MODULE_USAGE_COMMAND("factorytest"); -#endif - PRINT_MODULE_USAGE_COMMAND("testerror"); - - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -void -MPU6000::custom_method(const BusCLIArguments &cli) -{ - switch (cli.custom1) { - case 0: reset(); - break; - - case 1: print_registers(); - break; -#ifndef CONSTRAINED_FLASH - - case 2: factory_self_test(); - break; -#endif - - case 3: test_error(); - break; - } -} - -I2CSPIDriverBase *MPU6000::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - device::Device *interface = nullptr; - int device_type = cli.type; - - if (iterator.busType() == BOARD_I2C_BUS) { - interface = MPU6000_I2C_interface(iterator.bus(), iterator.devid(), device_type, iterator.external(), - cli.bus_frequency); - - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = MPU6000_SPI_interface(iterator.bus(), iterator.devid(), device_type, iterator.external(), - cli.bus_frequency, cli.spi_mode); - } - - if (interface == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (interface->init() != OK) { - delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - MPU6000 *dev = new MPU6000(interface, cli.rotation, device_type, iterator.configuredBusOption(), iterator.bus()); - - if (dev == nullptr) { - delete interface; - return nullptr; - } - - if (OK != dev->init()) { - delete dev; - return nullptr; - } - - dev->start(); - return dev; -} - -/** driver 'main' command */ -extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } - -int -mpu6000_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = MPU6000; - BusCLIArguments cli{true, true}; - cli.type = MPU_DEVICE_TYPE_MPU6000; - cli.default_i2c_frequency = 400000; - cli.default_spi_frequency = 1000 * 1000; // low speed bus frequency - - while ((ch = cli.getopt(argc, argv, "T:R:")) != EOF) { - switch (ch) { - case 'T': - cli.type = atoi(cli.optarg()); - break; - - case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); - break; - } - } - - const char *verb = cli.optarg(); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - uint16_t dev_type_driver = 0; - - switch (cli.type) { - case MPU_DEVICE_TYPE_MPU6000: - dev_type_driver = DRV_IMU_DEVTYPE_MPU6000; - break; - - case MPU_DEVICE_TYPE_ICM20602: - dev_type_driver = DRV_IMU_DEVTYPE_ICM20602; - break; - - case MPU_DEVICE_TYPE_ICM20608: - dev_type_driver = DRV_IMU_DEVTYPE_ICM20608G; - break; - - case MPU_DEVICE_TYPE_ICM20689: - dev_type_driver = DRV_IMU_DEVTYPE_ICM20689; - break; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, dev_type_driver); - - if (!strcmp(verb, "start")) { - return ThisDriver::module_start(cli, iterator); - } - - if (!strcmp(verb, "stop")) { - return ThisDriver::module_stop(iterator); - } - - if (!strcmp(verb, "status")) { - 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); - } - -#ifndef CONSTRAINED_FLASH - - if (!strcmp(verb, "factorytest")) { - cli.custom1 = 2; - return ThisDriver::module_custom_method(cli, iterator); - } - -#endif - - if (!strcmp(verb, "testerror")) { - cli.custom1 = 3; - return ThisDriver::module_custom_method(cli, iterator); - } - - ThisDriver::print_usage(); - return -1; -}