From 8dc050998943a8c8d7fcd91b985842dbade2378d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Jan 2019 10:41:47 -0500 Subject: [PATCH] mpu9250: split icm20948 support out into new separate driver --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- boards/px4/fmu-v5/default.cmake | 1 + src/drivers/imu/CMakeLists.txt | 1 + src/drivers/imu/icm20948/CMakeLists.txt | 48 + src/drivers/imu/icm20948/accel.cpp | 147 +++ src/drivers/imu/icm20948/accel.h | 63 + src/drivers/imu/icm20948/gyro.cpp | 112 ++ src/drivers/imu/icm20948/gyro.h | 62 + src/drivers/imu/icm20948/icm20948.cpp | 1432 +++++++++++++++++++++ src/drivers/imu/icm20948/icm20948.h | 684 ++++++++++ src/drivers/imu/icm20948/icm20948_i2c.cpp | 131 ++ src/drivers/imu/icm20948/icm20948_spi.cpp | 193 +++ src/drivers/imu/icm20948/mag.cpp | 513 ++++++++ src/drivers/imu/icm20948/mag.h | 194 +++ src/drivers/imu/icm20948/mag_i2c.cpp | 116 ++ src/drivers/imu/icm20948/main.cpp | 429 ++++++ src/drivers/imu/mpu9250/mag.cpp | 119 +- src/drivers/imu/mpu9250/mag.h | 5 - src/drivers/imu/mpu9250/main.cpp | 5 - src/drivers/imu/mpu9250/mpu9250.cpp | 336 +---- src/drivers/imu/mpu9250/mpu9250.h | 145 +-- src/drivers/imu/mpu9250/mpu9250_i2c.cpp | 14 - 22 files changed, 4200 insertions(+), 552 deletions(-) create mode 100644 src/drivers/imu/icm20948/CMakeLists.txt create mode 100644 src/drivers/imu/icm20948/accel.cpp create mode 100644 src/drivers/imu/icm20948/accel.h create mode 100644 src/drivers/imu/icm20948/gyro.cpp create mode 100644 src/drivers/imu/icm20948/gyro.h create mode 100644 src/drivers/imu/icm20948/icm20948.cpp create mode 100644 src/drivers/imu/icm20948/icm20948.h create mode 100644 src/drivers/imu/icm20948/icm20948_i2c.cpp create mode 100644 src/drivers/imu/icm20948/icm20948_spi.cpp create mode 100644 src/drivers/imu/icm20948/mag.cpp create mode 100644 src/drivers/imu/icm20948/mag.h create mode 100644 src/drivers/imu/icm20948/mag_i2c.cpp create mode 100644 src/drivers/imu/icm20948/main.cpp diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8106baeee6..947599affc 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -106,7 +106,7 @@ then fi # ICM20948 as external magnetometer on I2C (e.g. Here GPS) -mpu9250 -X -M -R 6 start +icm20948 -X -M -R 6 start # Check for flow sensor, launched as a background task to scan px4flow start & diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index abefe80754..862559a367 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -32,6 +32,7 @@ px4_add_board( imu/bmi160 imu/mpu6000 imu/mpu9250 + imu/icm20948 irlock lights/blinkm lights/oreoled diff --git a/src/drivers/imu/CMakeLists.txt b/src/drivers/imu/CMakeLists.txt index cdc81ea06e..ef5b8b6c8a 100644 --- a/src/drivers/imu/CMakeLists.txt +++ b/src/drivers/imu/CMakeLists.txt @@ -38,6 +38,7 @@ add_subdirectory(bmi055) add_subdirectory(bmi160) add_subdirectory(fxas21002c) add_subdirectory(fxos8701cq) +add_subdirectory(icm20948) add_subdirectory(l3gd20) add_subdirectory(lsm303d) add_subdirectory(mpu6000) diff --git a/src/drivers/imu/icm20948/CMakeLists.txt b/src/drivers/imu/icm20948/CMakeLists.txt new file mode 100644 index 0000000000..e32a67fa37 --- /dev/null +++ b/src/drivers/imu/icm20948/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__icm20948 + MAIN icm20948 + STACK_MAIN 1500 + COMPILE_FLAGS + SRCS + icm20948.cpp + icm20948_i2c.cpp + icm20948_spi.cpp + main.cpp + accel.cpp + gyro.cpp + mag.cpp + mag_i2c.cpp + DEPENDS + ) diff --git a/src/drivers/imu/icm20948/accel.cpp b/src/drivers/imu/icm20948/accel.cpp new file mode 100644 index 0000000000..a0e8fde252 --- /dev/null +++ b/src/drivers/imu/icm20948/accel.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * 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 gyro.cpp + * + * Driver for the Invensense mpu9250 connected via SPI. + * + * @author Andrew Tridgell + * + * based on the mpu6000 driver + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mag.h" +#include "gyro.h" +#include "icm20948.h" + +ICM20948_accel::ICM20948_accel(ICM20948 *parent, const char *path) : + CDev("ICM20948_accel", path), + _parent(parent) +{ +} + +ICM20948_accel::~ICM20948_accel() +{ + if (_accel_class_instance != -1) { + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } +} + +int +ICM20948_accel::init() +{ + // do base class init + int ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("accel init failed"); + return ret; + } + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + return ret; +} + +void +ICM20948_accel::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +int +ICM20948_accel::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* + * Repeated in ICM20948_mag::ioctl + * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 + */ + + switch (cmd) { + case SENSORIOCRESET: { + return _parent->reset(); + } + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* zero would be bad */ + case 0: + return -EINVAL; + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: + return _parent->_set_pollrate(arg); + } + } + + case ACCELIOCSSCALE: { + struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + memcpy(&_parent->_accel_scale, s, sizeof(_parent->_accel_scale)); + return OK; + } + + default: + return CDev::ioctl(filp, cmd, arg); + } +} diff --git a/src/drivers/imu/icm20948/accel.h b/src/drivers/imu/icm20948/accel.h new file mode 100644 index 0000000000..b31cf49aa4 --- /dev/null +++ b/src/drivers/imu/icm20948/accel.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +class ICM20948; + +/** + * Helper class implementing the accel driver node. + */ +class ICM20948_accel : public device::CDev +{ +public: + ICM20948_accel(ICM20948 *parent, const char *path); + ~ICM20948_accel(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class ICM20948; + + void parent_poll_notify(); + +private: + ICM20948 *_parent; + + orb_advert_t _accel_topic{nullptr}; + int _accel_orb_class_instance{-1}; + int _accel_class_instance{-1}; + +}; diff --git a/src/drivers/imu/icm20948/gyro.cpp b/src/drivers/imu/icm20948/gyro.cpp new file mode 100644 index 0000000000..2a1ec75d73 --- /dev/null +++ b/src/drivers/imu/icm20948/gyro.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 gyro.cpp + * + * Driver for the Invensense icm20948 connected via SPI. + * + * + * based on the mpu9250 driver + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mag.h" +#include "gyro.h" +#include "icm20948.h" + +ICM20948_gyro::ICM20948_gyro(ICM20948 *parent, const char *path) : + CDev("ICM20948_gyro", path), + _parent(parent) +{ +} + +ICM20948_gyro::~ICM20948_gyro() +{ + if (_gyro_class_instance != -1) { + unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); + } +} + +int +ICM20948_gyro::init() +{ + // do base class init + int ret = CDev::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); + + return ret; +} + +void +ICM20948_gyro::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +int +ICM20948_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + /* these are shared with the accel side */ + case SENSORIOCSPOLLRATE: + case SENSORIOCRESET: + return _parent->_accel->ioctl(filp, cmd, arg); + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_parent->_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_parent->_gyro_scale)); + return OK; + + default: + return CDev::ioctl(filp, cmd, arg); + } +} diff --git a/src/drivers/imu/icm20948/gyro.h b/src/drivers/imu/icm20948/gyro.h new file mode 100644 index 0000000000..02b47c25f2 --- /dev/null +++ b/src/drivers/imu/icm20948/gyro.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +class ICM20948; + +/** + * Helper class implementing the gyro driver node. + */ +class ICM20948_gyro : public device::CDev +{ +public: + ICM20948_gyro(ICM20948 *parent, const char *path); + ~ICM20948_gyro(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + virtual int init(); + +protected: + friend class ICM20948; + + void parent_poll_notify(); + +private: + ICM20948 *_parent; + + orb_advert_t _gyro_topic{nullptr}; + int _gyro_orb_class_instance{-1}; + int _gyro_class_instance{-1}; +}; diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp new file mode 100644 index 0000000000..6467bc9039 --- /dev/null +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -0,0 +1,1432 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file mpu9250.cpp + * + * Driver for the Invensense ICM20948 connected via I2C or SPI. + * + * + * based on the mpu9250 driver + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mag.h" +#include "accel.h" +#include "gyro.h" +#include "icm20948.h" + +/* + 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 + */ +#define MPU9250_TIMER_REDUCTION 200 + +/* Set accel range used */ +#define ACCEL_RANGE_G 16 +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { + ICMREG_20948_USER_CTRL, + ICMREG_20948_PWR_MGMT_1, + ICMREG_20948_PWR_MGMT_2, + ICMREG_20948_INT_PIN_CFG, + ICMREG_20948_INT_ENABLE, + ICMREG_20948_INT_ENABLE_1, + ICMREG_20948_INT_ENABLE_2, + ICMREG_20948_INT_ENABLE_3, + ICMREG_20948_GYRO_SMPLRT_DIV, + ICMREG_20948_GYRO_CONFIG_1, + ICMREG_20948_GYRO_CONFIG_2, + ICMREG_20948_ACCEL_SMPLRT_DIV_1, + ICMREG_20948_ACCEL_SMPLRT_DIV_2, + ICMREG_20948_ACCEL_CONFIG, + ICMREG_20948_ACCEL_CONFIG_2 +}; + + +ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, + const char *path_gyro, const char *path_mag, + enum Rotation rotation, + bool magnetometer_only) : + _interface(interface), + _whoami(0), + _accel(magnetometer_only ? nullptr : new ICM20948_accel(this, path_accel)), + _gyro(magnetometer_only ? nullptr : new ICM20948_gyro(this, path_gyro)), + _mag(new ICM20948_mag(this, mag_interface, path_mag)), + _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write + _magnetometer_only(magnetometer_only), +#if defined(USE_I2C) + _work {}, + _use_hrt(false), +#else + _use_hrt(true), +#endif + _call {}, + _call_interval(0), + _accel_reports(nullptr), + _accel_scale{}, + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(nullptr), + _gyro_reports(nullptr), + _gyro_scale{}, + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), + _dlpf_freq_icm_gyro(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), + _dlpf_freq_icm_accel(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), + _sample_rate(1000), + _accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")), + _gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")), + _sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")), + _bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_trans")), + _bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_reg")), + _good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_trans")), + _reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset")), + _duplicates(perf_alloc(PC_COUNT, "mpu9250_dupe")), + _register_wait(0), + _reset_wait(0), + _accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE), + _gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true), + _rotation(rotation), + _checked_registers(nullptr), + _checked_next(0), + _num_checked_registers(0), + _last_temperature(0), + _last_accel_data{}, + _got_duplicate(false) +{ + if (_accel != nullptr) { + /* Set device parameters and make sure parameters of the bus device are adopted */ + _accel->_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; + _accel->_device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type(); + _accel->_device_id.devid_s.bus = _interface->get_device_bus(); + _accel->_device_id.devid_s.address = _interface->get_device_address(); + } + + if (_gyro != nullptr) { + /* Prime _gyro with common devid. */ + /* Set device parameters and make sure parameters of the bus device are adopted */ + _gyro->_device_id.devid = 0; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250; + _gyro->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); + _gyro->_device_id.devid_s.bus = _interface->get_device_bus(); + _gyro->_device_id.devid_s.address = _interface->get_device_address(); + } + + /* Prime _mag with common devid. */ + _mag->_device_id.devid = 0; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; + _mag->_device_id.devid_s.bus_type = _interface->get_device_bus_type(); + _mag->_device_id.devid_s.bus = _interface->get_device_bus(); + _mag->_device_id.devid_s.address = _interface->get_device_address(); + + // default accel scale factors + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + // default gyro scale factors + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; +} + +ICM20948::~ICM20948() +{ + /* make sure we are truly inactive */ + stop(); + _call_interval = 0; + + if (!_magnetometer_only) { + orb_unadvertise(_accel_topic); + orb_unadvertise(_gyro->_gyro_topic); + } + + /* delete the accel subdriver */ + delete _accel; + + /* delete the gyro subdriver */ + delete _gyro; + + /* delete the magnetometer subdriver */ + delete _mag; + + /* free any existing reports */ + if (_accel_reports != nullptr) { + delete _accel_reports; + } + + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_accel_reads); + perf_free(_gyro_reads); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_good_transfers); + perf_free(_reset_retries); + perf_free(_duplicates); +} + +int +ICM20948::init() +{ + irqstate_t state; + +#if defined(USE_I2C) + use_i2c(_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); +#endif + + /* + * If the MPU is using I2C we should reduce the sample rate to 200Hz and + * make the integration autoreset faster so that we integrate just one + * sample since the sampling rate is already low. + */ + if (is_i2c() && !_magnetometer_only) { + _sample_rate = 200; + _accel_int.set_autoreset_interval(1000000 / 1000); + _gyro_int.set_autoreset_interval(1000000 / 1000); + } + + int ret = probe(); + + if (ret != OK) { + PX4_DEBUG("ICM20948 probe failed"); + return ret; + } + + state = px4_enter_critical_section(); + _reset_wait = hrt_absolute_time() + 100000; + px4_leave_critical_section(state); + + if (reset_mpu() != OK) { + PX4_ERR("Exiting! Device failed to take initialization"); + return ret; + } + + if (!_magnetometer_only) { + /* allocate basic report buffers */ + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); + ret = -ENOMEM; + + if (_accel_reports == nullptr) { + return ret; + } + + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); + + if (_gyro_reports == nullptr) { + return ret; + } + + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + // set software low pass filter for controllers + param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF"); + float accel_cut = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; + + if (accel_cut_ph != PARAM_INVALID && (param_get(accel_cut_ph, &accel_cut) == PX4_OK)) { + PX4_INFO("accel cutoff set to %.2f Hz", double(accel_cut)); + + _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); + _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); + _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, accel_cut); + + } + + param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF"); + float gyro_cut = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ; + + if (gyro_cut_ph != PARAM_INVALID && (param_get(gyro_cut_ph, &gyro_cut) == PX4_OK)) { + PX4_INFO("gyro cutoff set to %.2f Hz", double(gyro_cut)); + + _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); + _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); + _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, gyro_cut); + + } + + /* do CDev init for the accel device node */ + ret = _accel->init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + PX4_DEBUG("accel init failed"); + return ret; + } + + /* do CDev init for the gyro device node */ + ret = _gyro->init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + PX4_DEBUG("gyro init failed"); + return ret; + } + } + + /* Magnetometer setup */ + +#ifdef USE_I2C + up_udelay(100); + + if (!_mag->is_passthrough() && _mag->_interface->init() != PX4_OK) { + PX4_ERR("failed to setup ak8963 interface"); + } + +#endif /* USE_I2C */ + + /* do CDev init for the mag device node */ + ret = _mag->init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + PX4_DEBUG("mag init failed"); + return ret; + } + + ret = _mag->ak8963_reset(); + + if (ret != OK) { + PX4_DEBUG("mag reset failed"); + return ret; + } + + measure(); + + if (!_magnetometer_only) { + /* advertise sensor topic, measure manually to initialize valid report */ + sensor_accel_s arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel->_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + + if (_accel_topic == nullptr) { + PX4_ERR("ADVERT FAIL"); + return ret; + } + + /* advertise sensor topic, measure manually to initialize valid report */ + sensor_gyro_s grp; + _gyro_reports->get(&grp); + + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + + if (_gyro->_gyro_topic == nullptr) { + PX4_ERR("ADVERT FAIL"); + return ret; + } + } + + return ret; +} + +uint8_t ICM20948::get_whoami() +{ + return _whoami; +} + +int ICM20948::reset() +{ + irqstate_t state; + + /* When the mpu9250 starts from 0V the internal power on circuit + * per the data sheet will require: + * + * Start-up time for register read/write From power-up Typ:11 max:100 ms + * + */ + + px4_usleep(110000); + + // Hold off sampling until done (100 MS will be shortened) + state = px4_enter_critical_section(); + _reset_wait = hrt_absolute_time() + 100000; + px4_leave_critical_section(state); + + int ret; + + ret = reset_mpu(); + + if (ret == OK && (_whoami == ICM_WHOAMI_20948)) { + ret = _mag->ak8963_reset(); + } + + state = px4_enter_critical_section(); + _reset_wait = hrt_absolute_time() + 10; + px4_leave_critical_section(state); + + return ret; +} + +int ICM20948::reset_mpu() +{ + uint8_t retries; + + switch (_whoami) { + case ICM_WHOAMI_20948: + write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET); + usleep(1000); + + write_checked_reg(ICMREG_20948_PWR_MGMT_1, MPU_CLK_SEL_AUTO); + usleep(200); + write_checked_reg(ICMREG_20948_PWR_MGMT_2, 0); + break; + } + + // Enable I2C bus or Disable I2C bus (recommended on data sheet) + + + write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), is_i2c() ? 0 : BIT_I2C_IF_DIS); + + + // SAMPLE RATE + _set_sample_rate(_sample_rate); + + _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); + + // Gyro scale 2000 deg/s () + switch (_whoami) { + case ICM_WHOAMI_20948: + modify_checked_reg(ICMREG_20948_GYRO_CONFIG_1, ICM_BITS_GYRO_FS_SEL_MASK, ICM_BITS_GYRO_FS_SEL_2000DPS); + break; + } + + + // 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 + _gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); + _gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F; + + set_accel_range(ACCEL_RANGE_G); + + // INT CFG => Interrupt on Data Ready + write_checked_reg(MPU_OR_ICM(MPUREG_INT_ENABLE, ICMREG_20948_INT_ENABLE_1), + BIT_RAW_RDY_EN); // INT: Raw data ready + +#ifdef USE_I2C + bool bypass = !_mag->is_passthrough(); +#else + bool bypass = false; +#endif + + /* INT: Clear on any read. + * If this instance is for a device is on I2C bus the Mag will have an i2c interface + * that it will use to access the either: a) the internal mag device on the internal I2C bus + * or b) it could be used to access a downstream I2C devices connected to the chip on + * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master + * controller that chip provides as a SPI to I2C bridge. + * so bypass is true if the mag has an i2c non null interfaces. + */ + + write_checked_reg(MPU_OR_ICM(MPUREG_INT_PIN_CFG, ICMREG_20948_INT_PIN_CFG), + BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); + + write_checked_reg(MPU_OR_ICM(MPUREG_ACCEL_CONFIG2, ICMREG_20948_ACCEL_CONFIG_2), + MPU_OR_ICM(BITS_ACCEL_CONFIG2_41HZ, ICM_BITS_DEC3_CFG_32)); + + retries = 3; + bool all_ok = false; + + while (!all_ok && retries--) { + + // Assume all checked values are as expected + all_ok = true; + uint8_t reg; + uint8_t bankcheck = 0; + + for (uint8_t i = 0; i < _num_checked_registers; i++) { + if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { + if (_whoami == ICM_WHOAMI_20948) { + _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1); + } + + write_reg(_checked_registers[i], _checked_values[i]); + PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, + REG_BANK(_checked_registers[i]), bankcheck); + all_ok = false; + } + } + } + + return all_ok ? OK : -EIO; +} + +int +ICM20948::probe() +{ + int ret = PX4_ERROR; + + // Try first for mpu9250/6500 + _whoami = read_reg(MPUREG_WHOAMI); + + // must be an ICM + // Make sure selected register bank is bank 0 (which contains WHOAMI) + select_register_bank(REG_BANK(ICMREG_20948_WHOAMI)); + _whoami = read_reg(ICMREG_20948_WHOAMI); + + if (_whoami == ICM_WHOAMI_20948) { + _num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS; + _checked_registers = _icm20948_checked_registers; + memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS); + memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS); + ret = PX4_OK; + } + + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + + if (ret != PX4_OK) { + PX4_DEBUG("unexpected whoami 0x%02x", _whoami); + } + + return ret; +} + +/* + set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro + For ICM20948 accel and gyro samplerates are both set to the same value. +*/ +void +ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz) +{ + uint8_t div = 1; + + if (desired_sample_rate_hz == 0) { + desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; + } + + switch (_whoami) { + case ICM_WHOAMI_20948: + div = 1100 / desired_sample_rate_hz; + break; + } + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + + switch (_whoami) { + case ICM_WHOAMI_20948: + write_checked_reg(ICMREG_20948_GYRO_SMPLRT_DIV, div - 1); + // There's also an MSB for this allowing much higher dividers for the accelerometer. + // For 1 < div < 200 the LSB is sufficient. + write_checked_reg(ICMREG_20948_ACCEL_SMPLRT_DIV_2, div - 1); + _sample_rate = 1100 / div; + break; + } +} + +/* + * Set poll rate + */ +int +ICM20948::_set_pollrate(unsigned long rate) +{ + if (rate == 0) { + return -EINVAL; + + } else { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / rate; + + /* check against maximum sane rate */ + if (ticks < 1000) { + return -EINVAL; + } + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f / ticks; + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + + float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_interval = ticks; + + /* + set call interval faster than the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu9250 clock + */ + _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } +} + +/* + set the DLPF filter frequency. This affects both accel and gyro. + */ +void +ICM20948::_set_dlpf_filter(uint16_t frequency_hz) +{ + uint8_t filter; + + switch (_whoami) { + case ICM_WHOAMI_20948: + + /* + choose next highest filter frequency available for gyroscope + */ + if (frequency_hz == 0) { + _dlpf_freq_icm_gyro = 0; + filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; + + } else if (frequency_hz <= 5) { + _dlpf_freq_icm_gyro = 5; + filter = ICM_BITS_GYRO_DLPF_CFG_5HZ; + + } else if (frequency_hz <= 11) { + _dlpf_freq_icm_gyro = 11; + filter = ICM_BITS_GYRO_DLPF_CFG_11HZ; + + } else if (frequency_hz <= 23) { + _dlpf_freq_icm_gyro = 23; + filter = ICM_BITS_GYRO_DLPF_CFG_23HZ; + + } else if (frequency_hz <= 51) { + _dlpf_freq_icm_gyro = 51; + filter = ICM_BITS_GYRO_DLPF_CFG_51HZ; + + } else if (frequency_hz <= 119) { + _dlpf_freq_icm_gyro = 119; + filter = ICM_BITS_GYRO_DLPF_CFG_119HZ; + + } else if (frequency_hz <= 151) { + _dlpf_freq_icm_gyro = 151; + filter = ICM_BITS_GYRO_DLPF_CFG_151HZ; + + } else if (frequency_hz <= 197) { + _dlpf_freq_icm_gyro = 197; + filter = ICM_BITS_GYRO_DLPF_CFG_197HZ; + + } else { + _dlpf_freq_icm_gyro = 0; + filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; + } + + write_checked_reg(ICMREG_20948_GYRO_CONFIG_1, filter); + + /* + choose next highest filter frequency available for accelerometer + */ + if (frequency_hz == 0) { + _dlpf_freq_icm_accel = 0; + filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; + + } else if (frequency_hz <= 5) { + _dlpf_freq_icm_accel = 5; + filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ; + + } else if (frequency_hz <= 11) { + _dlpf_freq_icm_accel = 11; + filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ; + + } else if (frequency_hz <= 23) { + _dlpf_freq_icm_accel = 23; + filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ; + + } else if (frequency_hz <= 50) { + _dlpf_freq_icm_accel = 50; + filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ; + + } else if (frequency_hz <= 111) { + _dlpf_freq_icm_accel = 111; + filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ; + + } else if (frequency_hz <= 246) { + _dlpf_freq_icm_accel = 246; + filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ; + + } else { + _dlpf_freq_icm_accel = 0; + filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; + } + + write_checked_reg(ICMREG_20948_ACCEL_CONFIG, filter); + break; + } +} + +int +ICM20948::select_register_bank(uint8_t bank) +{ + uint8_t ret; + uint8_t buf; + uint8_t retries = 3; + + if (_selected_bank != bank) { + ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); + + if (ret != OK) { + return ret; + } + } + + /* + * Making sure the right register bank is selected (even if it should be). Observed some + * unexpected changes to this, don't risk writing to the wrong register bank. + */ + _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); + + while (bank != buf && retries > 0) { + //PX4_WARN("user bank: expected %d got %d",bank,buf); + ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); + + if (ret != OK) { + return ret; + } + + retries--; + //PX4_WARN("BANK retries: %d", 4-retries); + + _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); + } + + + _selected_bank = bank; + + if (bank != buf) { + PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf); + return PX4_ERROR; + + } else { + return PX4_OK; + } +} + +uint8_t +ICM20948::read_reg(unsigned reg, uint32_t speed) +{ + uint8_t buf; + + if (_whoami == ICM_WHOAMI_20948) { + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + + } else { + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + } + + return buf; +} + +uint8_t +ICM20948::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) +{ + uint8_t ret; + + if (buf == NULL) { return PX4_ERROR; } + + if (_whoami == ICM_WHOAMI_20948) { + select_register_bank(REG_BANK(start_reg)); + ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + + } else { + ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + } + + return ret; +} + +uint16_t +ICM20948::read_reg16(unsigned reg) +{ + uint8_t buf[2]; + + // general register transfer at low clock speed + + if (_whoami == ICM_WHOAMI_20948) { + select_register_bank(REG_BANK(reg)); + _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); + + } else { + _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); + } + + return (uint16_t)(buf[0] << 8) | buf[1]; +} + +void +ICM20948::write_reg(unsigned reg, uint8_t value) +{ + // general register transfer at low clock speed + + if (_whoami == ICM_WHOAMI_20948) { + select_register_bank(REG_BANK(reg)); + _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); + + } else { + _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); + } +} + +void +ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_reg(reg, val); +} + +void +ICM20948::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +ICM20948::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < _num_checked_registers; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + break; + } + } +} + +int +ICM20948::set_accel_range(unsigned max_g_in) +{ + 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; + } + + switch (_whoami) { + case ICM_WHOAMI_20948: + modify_checked_reg(ICMREG_20948_ACCEL_CONFIG, ICM_BITS_ACCEL_FS_SEL_MASK, afs_sel << 1); + break; + } + + _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; + + return OK; +} + +void +ICM20948::start() +{ + /* make sure we are stopped first */ + stop(); + + /* discard any stale data in the buffers */ + if (!_magnetometer_only) { + _accel_reports->flush(); + _gyro_reports->flush(); + } + + _mag->_mag_reports->flush(); + + if (_use_hrt) { + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - MPU9250_TIMER_REDUCTION, + (hrt_callout)&ICM20948::measure_trampoline, this); + + } else { +#ifdef USE_I2C + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&ICM20948::cycle_trampoline, this, 1); +#endif + } + +} + +void +ICM20948::stop() +{ + if (_use_hrt) { + hrt_cancel(&_call); + + } else { +#ifdef USE_I2C + work_cancel(HPWORK, &_work); +#endif + } +} + + +#if defined(USE_I2C) +void +ICM20948::cycle_trampoline(void *arg) +{ + ICM20948 *dev = (ICM20948 *)arg; + + dev->cycle(); +} + +void +ICM20948::cycle() +{ + +// int ret = measure(); + + measure(); + +// if (ret != OK) { +// /* issue a reset command to the sensor */ +// reset(); +// start(); +// return; +// } + + if (_call_interval != 0) { + work_queue(HPWORK, + &_work, + (worker_t)&ICM20948::cycle_trampoline, + this, + USEC2TICK(_call_interval - MPU9250_TIMER_REDUCTION)); + } +} +#endif + + +void +ICM20948::measure_trampoline(void *arg) +{ + ICM20948 *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +ICM20948::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 propagation 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; + + if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + 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 == 0) { + // if the product_id is wrong then reset the + // sensor completely + + if (_whoami == ICM_WHOAMI_20948) { + // reset_mpu(); + } else { + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); + } + + // 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 mpu9250 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) % _num_checked_registers; +} + +bool ICM20948::check_null_data(uint16_t *data, uint8_t size) +{ + while (size--) { + if (*data++) { + perf_count(_good_transfers); + return false; + } + } + + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu9250 does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return true; +} + +bool ICM20948::check_duplicate(uint8_t *accel_data) +{ + /* + 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(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) { + // it isn't new data - wait for next timer + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + + } else { + memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data)); + _got_duplicate = false; + } + + return _got_duplicate; +} + +void +ICM20948::measure() +{ + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct MPUReport mpu_report; + + struct ICMReport icm_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 MPU9250 in one pass + */ + + if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) { + + select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); + + if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report, + sizeof(icm_report))) { + perf_end(_sample_perf); + return; + } + + check_registers(); + + if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) { + return; + } + } + + /* + * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else, + * try to read a magnetometer report. + */ + +# ifdef USE_I2C + + if (_mag->is_passthrough()) { +# endif + + _mag->_measure(mpu_report.mag); + +# ifdef USE_I2C + + } else { + _mag->measure(); + } + +# endif + + /* + * Continue evaluating gyro and accelerometer results + */ + if (!_magnetometer_only && _register_wait == 0) { + + /* + * Convert from big to little endian + */ + if (_whoami == ICM_WHOAMI_20948) { + report.accel_x = int16_t_from_bytes(icm_report.accel_x); + report.accel_y = int16_t_from_bytes(icm_report.accel_y); + report.accel_z = int16_t_from_bytes(icm_report.accel_z); + report.temp = int16_t_from_bytes(icm_report.temp); + report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); + report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); + report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); + + } else { // MPU9250/MPU6500 + 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 (check_null_data((uint16_t *)&report, sizeof(report) / 2)) { + return; + } + } + + if (_register_wait != 0) { + /* + * We are waiting for some good transfers before using the sensor again. + * We still increment _good_transfers, but don't return any data yet. + * + */ + _register_wait--; + return; + } + + /* + * Get sensor temperature + */ + _last_temperature = (report.temp) / 333.87f + 21.0f; + + + /* + * Convert and publish accelerometer and gyrometer data. + */ + + if (!_magnetometer_only) { + + /* + * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation + */ + if (_whoami != ICM_WHOAMI_20948) { + /* + * 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 buffers. + */ + sensor_accel_s arb; + sensor_gyro_s grb; + + /* + * Adjust and scale results to m/s^2. + */ + grb.timestamp = 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 + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + + /* + * 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. + */ + + /* NOTE: Axes have been swapped to match the board a few lines above. */ + + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; + + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); + + matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); + matrix::Vector3f aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + + arb.scaling = _accel_range_scale; + + arb.temperature = _last_temperature; + + /* return device ID */ + arb.device_id = _accel->_device_id.devid; + + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; + + xraw_f = report.gyro_x; + yraw_f = report.gyro_y; + zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); + + matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + matrix::Vector3f gval_integrated; + + bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + + grb.scaling = _gyro_range_scale; + + grb.temperature = _last_temperature; + + /* return device ID */ + grb.device_id = _gyro->_device_id.devid; + + _accel_reports->force(&arb); + _gyro_reports->force(&grb); + + /* notify anyone waiting for data */ + if (accel_notify) { + _accel->poll_notify(POLLIN); + } + + if (gyro_notify) { + _gyro->parent_poll_notify(); + } + + if (accel_notify && !(_accel->_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + + if (gyro_notify && !(_gyro->_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); + } + } + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +ICM20948::print_info() +{ + ::printf("Device type:%d\n", _whoami); + perf_print_counter(_sample_perf); + perf_print_counter(_accel_reads); + perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); + perf_print_counter(_duplicates); + ::printf("temperature: %.1f\n", (double)_last_temperature); + + if (!_magnetometer_only) { + _accel_reports->print_info("accel queue"); + _gyro_reports->print_info("gyro queue"); + _mag->_mag_reports->print_info("mag queue"); + } +} diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h new file mode 100644 index 0000000000..35586514ae --- /dev/null +++ b/src/drivers/imu/icm20948/icm20948.h @@ -0,0 +1,684 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "mag.h" +#include "accel.h" +#include "gyro.h" + + +#if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) +# define USE_I2C +#endif + + +// MPU 9250 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_ACCEL_CONFIG2 0x1D +#define MPUREG_LPACCEL_ODR 0x1E +#define MPUREG_WOM_THRESH 0x1F +#define MPUREG_FIFO_EN 0x23 +#define MPUREG_I2C_MST_CTRL 0x24 +#define MPUREG_I2C_SLV0_ADDR 0x25 +#define MPUREG_I2C_SLV0_REG 0x26 +#define MPUREG_I2C_SLV0_CTRL 0x27 +#define MPUREG_I2C_SLV1_ADDR 0x28 +#define MPUREG_I2C_SLV1_REG 0x29 +#define MPUREG_I2C_SLV1_CTRL 0x2A +#define MPUREG_I2C_SLV2_ADDR 0x2B +#define MPUREG_I2C_SLV2_REG 0x2C +#define MPUREG_I2C_SLV2_CTRL 0x2D +#define MPUREG_I2C_SLV3_ADDR 0x2E +#define MPUREG_I2C_SLV3_REG 0x2F +#define MPUREG_I2C_SLV3_CTRL 0x30 +#define MPUREG_I2C_SLV4_ADDR 0x31 +#define MPUREG_I2C_SLV4_REG 0x32 +#define MPUREG_I2C_SLV4_DO 0x33 +#define MPUREG_I2C_SLV4_CTRL 0x34 +#define MPUREG_I2C_SLV4_DI 0x35 +#define MPUREG_I2C_MST_STATUS 0x36 +#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_EXT_SENS_DATA_00 0x49 +#define MPUREG_I2C_SLV0_D0 0x63 +#define MPUREG_I2C_SLV1_D0 0x64 +#define MPUREG_I2C_SLV2_D0 0x65 +#define MPUREG_I2C_SLV3_D0 0x66 +#define MPUREG_I2C_MST_DELAY_CTRL 0x67 +#define MPUREG_SIGNAL_PATH_RESET 0x68 +#define MPUREG_MOT_DETECT_CTRL 0x69 +#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 + +// Configuration bits MPU 9250 +#define BIT_SLEEP 0x40 +#define BIT_H_RESET 0x80 +#define MPU_CLK_SEL_AUTO 0x01 + +#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 BITS_DLPF_CFG_250HZ 0x00 +#define BITS_DLPF_CFG_184HZ 0x01 +#define BITS_DLPF_CFG_92HZ 0x02 +#define BITS_DLPF_CFG_41HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_3600HZ 0x07 +#define BITS_DLPF_CFG_MASK 0x07 + +#define BITS_ACCEL_CONFIG2_41HZ 0x03 + +#define BIT_RAW_RDY_EN 0x01 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_INT_BYPASS_EN 0x02 + +#define BIT_I2C_READ_FLAG 0x80 + +#define BIT_I2C_SLV0_NACK 0x01 +#define BIT_I2C_FIFO_EN 0x40 +#define BIT_I2C_MST_EN 0x20 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_FIFO_RST 0x04 +#define BIT_I2C_MST_RST 0x02 +#define BIT_SIG_COND_RST 0x01 + +#define BIT_I2C_SLV0_EN 0x80 +#define BIT_I2C_SLV0_BYTE_SW 0x40 +#define BIT_I2C_SLV0_REG_DIS 0x20 +#define BIT_I2C_SLV0_REG_GRP 0x10 + +#define BIT_I2C_MST_MULT_MST_EN 0x80 +#define BIT_I2C_MST_WAIT_FOR_ES 0x40 +#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20 +#define BIT_I2C_MST_P_NSR 0x10 +#define BITS_I2C_MST_CLOCK_258HZ 0x08 +#define BITS_I2C_MST_CLOCK_400HZ 0x0D + +#define BIT_I2C_SLV0_DLY_EN 0x01 +#define BIT_I2C_SLV1_DLY_EN 0x02 +#define BIT_I2C_SLV2_DLY_EN 0x04 +#define BIT_I2C_SLV3_DLY_EN 0x08 + +#define ICM_WHOAMI_20948 0xEA + +#define MPU9250_ACCEL_DEFAULT_RATE 1000 +#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 +#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define MPU9250_GYRO_DEFAULT_RATE 1000 +/* rates need to be the same between accel and gyro */ +#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE +#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 + +#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92 + +#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) + + + +// ICM20948 registers and data + +/* + * ICM20948 I2C address LSB can be switched by the chip's AD0 pin, thus is device dependent. + * Noting this down for now. Here GPS uses 0x69. To support a device implementing the second + * address, probably an additional MPU_DEVICE_TYPE is the way to go. + */ +#define PX4_I2C_EXT_ICM20948_0 0x68 +#define PX4_I2C_EXT_ICM20948_1 0x69 + +/* + * ICM20948 uses register banks. Register 127 (0x7F) is used to switch between 4 banks. + * There's room in the upper address byte below the port speed setting to code in the + * used bank. This is a bit more efficient, already in use for the speed setting and more + * in one place than a solution with a lookup table for address/bank pairs. + */ + +#define BANK0 0x0000 +#define BANK1 0x0100 +#define BANK2 0x0200 +#define BANK3 0x0300 + +#define BANK_REG_MASK 0x0300 +#define REG_BANK(r) (((r) & BANK_REG_MASK)>>4) +#define REG_ADDRESS(r) ((r) & ~BANK_REG_MASK) + +#define ICMREG_20948_BANK_SEL 0x7F + +#define ICMREG_20948_WHOAMI (0x00 | BANK0) +#define ICMREG_20948_USER_CTRL (0x03 | BANK0) +#define ICMREG_20948_PWR_MGMT_1 (0x06 | BANK0) +#define ICMREG_20948_PWR_MGMT_2 (0x07 | BANK0) +#define ICMREG_20948_INT_PIN_CFG (0x0F | BANK0) +#define ICMREG_20948_INT_ENABLE (0x10 | BANK0) +#define ICMREG_20948_INT_ENABLE_1 (0x11 | BANK0) +#define ICMREG_20948_ACCEL_XOUT_H (0x2D | BANK0) +#define ICMREG_20948_INT_ENABLE_2 (0x12 | BANK0) +#define ICMREG_20948_INT_ENABLE_3 (0x13 | BANK0) +#define ICMREG_20948_EXT_SLV_SENS_DATA_00 (0x3B | BANK0) +#define ICMREG_20948_GYRO_SMPLRT_DIV (0x00 | BANK2) +#define ICMREG_20948_GYRO_CONFIG_1 (0x01 | BANK2) +#define ICMREG_20948_GYRO_CONFIG_2 (0x02 | BANK2) +#define ICMREG_20948_ACCEL_SMPLRT_DIV_1 (0x10 | BANK2) +#define ICMREG_20948_ACCEL_SMPLRT_DIV_2 (0x11 | BANK2) +#define ICMREG_20948_ACCEL_CONFIG (0x14 | BANK2) +#define ICMREG_20948_ACCEL_CONFIG_2 (0x15 | BANK2) +#define ICMREG_20948_I2C_MST_CTRL (0x01 | BANK3) +#define ICMREG_20948_I2C_SLV0_ADDR (0x03 | BANK3) +#define ICMREG_20948_I2C_SLV0_REG (0x04 | BANK3) +#define ICMREG_20948_I2C_SLV0_CTRL (0x05 | BANK3) +#define ICMREG_20948_I2C_SLV0_DO (0x06 | BANK3) + + + +/* +* ICM20948 register bits +* Most of the regiser set values from MPU9250 have the same +* meaning on ICM20948. The exceptions and values not already +* defined for MPU9250 are defined below +*/ +#define ICM_BIT_PWR_MGMT_1_ENABLE 0x00 +#define ICM_BIT_USER_CTRL_I2C_MST_DISABLE 0x00 + +#define ICM_BITS_GYRO_DLPF_CFG_197HZ 0x01 +#define ICM_BITS_GYRO_DLPF_CFG_151HZ 0x09 +#define ICM_BITS_GYRO_DLPF_CFG_119HZ 0x11 +#define ICM_BITS_GYRO_DLPF_CFG_51HZ 0x19 +#define ICM_BITS_GYRO_DLPF_CFG_23HZ 0x21 +#define ICM_BITS_GYRO_DLPF_CFG_11HZ 0x29 +#define ICM_BITS_GYRO_DLPF_CFG_5HZ 0x31 +#define ICM_BITS_GYRO_DLPF_CFG_361HZ 0x39 +#define ICM_BITS_GYRO_DLPF_CFG_MASK 0x39 + +#define ICM_BITS_GYRO_FS_SEL_250DPS 0x00 +#define ICM_BITS_GYRO_FS_SEL_500DPS 0x02 +#define ICM_BITS_GYRO_FS_SEL_1000DPS 0x04 +#define ICM_BITS_GYRO_FS_SEL_2000DPS 0x06 +#define ICM_BITS_GYRO_FS_SEL_MASK 0x06 + +#define ICM_BITS_ACCEL_DLPF_CFG_246HZ 0x09 +#define ICM_BITS_ACCEL_DLPF_CFG_111HZ 0x11 +#define ICM_BITS_ACCEL_DLPF_CFG_50HZ 0x19 +#define ICM_BITS_ACCEL_DLPF_CFG_23HZ 0x21 +#define ICM_BITS_ACCEL_DLPF_CFG_11HZ 0x29 +#define ICM_BITS_ACCEL_DLPF_CFG_5HZ 0x31 +#define ICM_BITS_ACCEL_DLPF_CFG_473HZ 0x39 +#define ICM_BITS_ACCEL_DLPF_CFG_MASK 0x39 + +#define ICM_BITS_ACCEL_FS_SEL_250DPS 0x00 +#define ICM_BITS_ACCEL_FS_SEL_500DPS 0x02 +#define ICM_BITS_ACCEL_FS_SEL_1000DPS 0x04 +#define ICM_BITS_ACCEL_FS_SEL_2000DPS 0x06 +#define ICM_BITS_ACCEL_FS_SEL_MASK 0x06 + +#define ICM_BITS_DEC3_CFG_4 0x00 +#define ICM_BITS_DEC3_CFG_8 0x01 +#define ICM_BITS_DEC3_CFG_16 0x10 +#define ICM_BITS_DEC3_CFG_32 0x11 +#define ICM_BITS_DEC3_CFG_MASK 0x11 + +#define ICM_BITS_I2C_MST_CLOCK_370KHZ 0x00 +#define ICM_BITS_I2C_MST_CLOCK_400HZ 0x07 // recommended by datasheet for 400kHz target clock + + + +#define MPU_OR_ICM(m,i) ((_whoami==ICM_WHOAMI_20948) ? i : m) + + +#pragma pack(push, 1) +/** + * Report conversation within the mpu, including command byte and + * interrupt status. + */ +struct ICMReport { + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + uint8_t temp[2]; + struct ak8963_regs mag; +}; +#pragma pack(pop) + + + + +#pragma pack(push, 1) +/** + * Report conversation within the mpu, 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]; + struct ak8963_regs mag; +}; +#pragma pack(pop) + +#define MPU_MAX_WRITE_BUFFER_SIZE (2) + + +/* + The MPU9250 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 MPU9250_LOW_BUS_SPEED 0 +#define MPU9250_HIGH_BUS_SPEED 0x8000 +#define MPU9250_REG_MASK 0x00FF +# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED) +# define MPU9250_REG(r) ((r) & MPU9250_REG_MASK) +# define MPU9250_SET_SPEED(r, s) ((r)|(s)) +# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED) +# define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED) + +/* interface factories */ +extern device::Device *ICM20948_SPI_interface(int bus, uint32_t cs, bool external_bus); +extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus); +extern int MPU9250_probe(device::Device *dev); + +typedef device::Device *(*ICM20948_constructor)(int, uint32_t, bool); + +class ICM20948_mag; +class ICM20948_accel; +class ICM20948_gyro; + +class ICM20948 +{ +public: + ICM20948(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, + const char *path_mag, + enum Rotation rotation, + bool magnetometer_only); + + virtual ~ICM20948(); + + virtual int init(); + uint8_t get_whoami(); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + device::Device *_interface; + uint8_t _whoami; /** whoami result */ + + virtual int probe(); + + friend class ICM20948_accel; + friend class ICM20948_mag; + friend class ICM20948_gyro; + +private: + ICM20948_accel *_accel; + ICM20948_gyro *_gyro; + ICM20948_mag *_mag; + uint8_t _selected_bank; /* Remember selected memory bank to avoid polling / setting on each read/write */ + bool + _magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */ + +#if defined(USE_I2C) + /* + * SPI bus based device use hrt + * I2C bus needs to use work queue + */ + work_s _work{}; +#endif + bool _use_hrt; + + struct hrt_call _call {}; + unsigned _call_interval; + + ringbuffer::RingBuffer *_accel_reports; + + struct accel_calibration_s _accel_scale; + float _accel_range_scale; + float _accel_range_m_s2; + orb_advert_t _accel_topic; + + ringbuffer::RingBuffer *_gyro_reports; + + struct gyro_calibration_s _gyro_scale; + float _gyro_range_scale; + float _gyro_range_rad_s; + + unsigned _dlpf_freq; + unsigned _dlpf_freq_icm_gyro; + unsigned _dlpf_freq_icm_accel; + + unsigned _sample_rate; + perf_counter_t _accel_reads; + perf_counter_t _gyro_reads; + 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; + + math::LowPassFilter2p _accel_filter_x; + math::LowPassFilter2p _accel_filter_y; + math::LowPassFilter2p _accel_filter_z; + math::LowPassFilter2p _gyro_filter_x; + math::LowPassFilter2p _gyro_filter_y; + math::LowPassFilter2p _gyro_filter_z; + + Integrator _accel_int; + Integrator _gyro_int; + + enum Rotation _rotation; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset + +#ifndef MAX +#define MAX(X,Y) ((X) > (Y) ? (X) : (Y)) +#endif + + static constexpr int ICM20948_NUM_CHECKED_REGISTERS{15}; + static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS]; + + const uint16_t *_checked_registers; + + uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS]; + unsigned _checked_next; + unsigned _num_checked_registers; + + + // last temperature reading for print_info() + float _last_temperature; + + bool check_null_data(uint16_t *data, uint8_t size); + bool check_duplicate(uint8_t *accel_data); + // keep last accel reading for duplicate detection + uint8_t _last_accel_data[6]; + 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(); + + + /** + * Resets the main chip (excluding the magnetometer if any). + */ + int reset_mpu(); + + +#if defined(USE_I2C) + /** + * When the I2C interfase is on + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + void use_i2c(bool on_true) { _use_hrt = !on_true; } + +#endif + + bool is_i2c(void) { return !_use_hrt; } + + + + + /** + * 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(); + + /** + * Select a register bank in ICM20948 + * + * Only actually switches if the remembered bank is different from the + * requested one + * + * @param The index of the register bank to switch to (0-3) + * @return Error code + */ + int select_register_bank(uint8_t bank); + + + /** + * Read a register from the mpu + * + * @param The register to read. + * @param The bus speed to read with. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); + uint16_t read_reg16(unsigned reg); + + + /** + * Read a register range from the mpu + * + * @param The start address to read from. + * @param The bus speed to read with. + * @param The address of the target data buffer. + * @param The count of bytes to be read. + * @return The value that was read. + */ + uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count); + + /** + * Write a register in the mpu + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_reg(unsigned reg, uint8_t value); + + /** + * Modify a register in the mpu + * + * 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 mpu, 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); + + /** + * Modify a checked register in the mpu + * + * 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_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + + /** + * Set the mpu 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); + + /** + * Swap a 16-bit value read from the mpu to native byte order. + */ + uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return _interface->external(); } + + /* + set low pass filter frequency + */ + void _set_dlpf_filter(uint16_t frequency_hz); + + /* + set sample rate (approximate) - 1kHz to 5Hz + */ + void _set_sample_rate(unsigned desired_sample_rate_hz); + + /* + set poll rate + */ + int _set_pollrate(unsigned long rate); + + /* + 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 */ + ICM20948(const ICM20948 &); + ICM20948 operator=(const ICM20948 &); +}; diff --git a/src/drivers/imu/icm20948/icm20948_i2c.cpp b/src/drivers/imu/icm20948/icm20948_i2c.cpp new file mode 100644 index 0000000000..6ac07de3c5 --- /dev/null +++ b/src/drivers/imu/icm20948/icm20948_i2c.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * 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 icm20948_i2c.cpp + * + * I2C interface for ICM20948 + */ + +#include +#include +#include +#include + +#include "icm20948.h" + +#ifdef USE_I2C + +device::Device *ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus); + +class ICM20948_I2C : public device::I2C +{ +public: + ICM20948_I2C(int bus, uint32_t address); + ~ICM20948_I2C() override = default; + + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; + +protected: + virtual int probe(); + +private: + +}; + +device::Device * +ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus) +{ + return new ICM20948_I2C(bus, address); +} + +ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address) : + I2C("ICM20948_I2C", nullptr, bus, address, 400000) +{ + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; +} + +int +ICM20948_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] = MPU9250_REG(reg_speed); + cmd[1] = *(uint8_t *)data; + return transfer(&cmd[0], count + 1, nullptr, 0); +} + +int +ICM20948_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 = MPU9250_REG(reg_speed); + return transfer(&cmd, 1, &((uint8_t *)data)[offset], count); +} + +int +ICM20948_I2C::probe() +{ + uint8_t whoami = 0; + uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948 + + // Try first for mpu9250/6500 + read(MPUREG_WHOAMI, &whoami, 1); + + /* + * If it's not an MPU it must be an ICM + * Make sure register bank 0 is selected - whoami is only present on bank 0, and that is + * not sure e.g. if the device has rebooted without repowering the sensor + */ + write(ICMREG_20948_BANK_SEL, ®ister_select, 1); + read(ICMREG_20948_WHOAMI, &whoami, 1); + + if (whoami == ICM_WHOAMI_20948) { + return PX4_OK; + } + + return -ENODEV; +} + +#endif /* USE_I2C */ diff --git a/src/drivers/imu/icm20948/icm20948_spi.cpp b/src/drivers/imu/icm20948/icm20948_spi.cpp new file mode 100644 index 0000000000..bd0730bfa3 --- /dev/null +++ b/src/drivers/imu/icm20948/icm20948_spi.cpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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 mpu9250_spi.cpp + * + * Driver for the Invensense ICM20948 connected via SPI. + * + * @author Andrew Tridgell + * @author Pat Hickey + * @author David sidrane + */ + +#include +#include +#include +#include + +#include "icm20948.h" + +#define DIR_READ 0x80 +#define DIR_WRITE 0x00 + +/* + * The ICM20948 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 Actual Value will be rounded down by the spi driver. + * for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU + * it will be 11.250 Mhz + */ +#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 +#define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 + +device::Device *ICM20948_SPI_interface(int bus, uint32_t cs, bool external_bus); + +class ICM20948_SPI : public device::SPI +{ +public: + ICM20948_SPI(int bus, uint32_t device); + ~ICM20948_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: + + /* Helper to set the desired speed and isolate the register on return */ + void set_bus_frequency(unsigned ®_speed_reg_out); +}; + +device::Device * +ICM20948_SPI_interface(int bus, uint32_t cs, bool external_bus) +{ + device::Device *interface = nullptr; + + if (external_bus) { +#if !(defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)) + errx(0, "External SPI not available"); +#endif + } + + if (cs != SPIDEV_NONE(0)) { + interface = new ICM20948_SPI(bus, cs); + } + + return interface; +} + +ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device) : + SPI("ICM20948", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED) +{ + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; +} + +void +ICM20948_SPI::set_bus_frequency(unsigned ®_speed) +{ + /* Set the desired speed */ + set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED); + + /* Isoolate the register on return */ + reg_speed = MPU9250_REG(reg_speed); +} + +int +ICM20948_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 +ICM20948_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; +} + +int +ICM20948_SPI::probe() +{ + uint8_t whoami = 0; + + int ret = read(MPUREG_WHOAMI, &whoami, 1); + + if (ret != OK) { + return -EIO; + } + + switch (whoami) { + default: + PX4_WARN("probe failed! %u", whoami); + ret = -EIO; + } + + return ret; +} diff --git a/src/drivers/imu/icm20948/mag.cpp b/src/drivers/imu/icm20948/mag.cpp new file mode 100644 index 0000000000..4d42173342 --- /dev/null +++ b/src/drivers/imu/icm20948/mag.cpp @@ -0,0 +1,513 @@ +/**************************************************************************** + * + * 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 mag.cpp + * + * Driver for the ak8963 magnetometer within the Invensense mpu9250 + * + * @author Robert Dickenson + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mag.h" +#include "icm20948.h" + + +// If interface is non-null, then it will used for interacting with the device. +// Otherwise, it will passthrough the parent ICM20948 +ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path) : + CDev("ICM20948_mag", path), + _interface(interface), + _parent(parent), + _mag_topic(nullptr), + _mag_orb_class_instance(-1), + _mag_class_instance(-1), + _mag_reading_data(false), + _mag_reports(nullptr), + _mag_scale{}, + _mag_range_scale(), + _mag_reads(perf_alloc(PC_COUNT, "mpu9250_mag_reads")), + _mag_errors(perf_alloc(PC_COUNT, "mpu9250_mag_errors")), + _mag_overruns(perf_alloc(PC_COUNT, "mpu9250_mag_overruns")), + _mag_overflows(perf_alloc(PC_COUNT, "mpu9250_mag_overflows")), + _mag_duplicates(perf_alloc(PC_COUNT, "mpu9250_mag_duplicates")), + _mag_asa_x(1.0), + _mag_asa_y(1.0), + _mag_asa_z(1.0), + _last_mag_data{} +{ + // default mag scale factors + _mag_scale.x_offset = 0; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0; + _mag_scale.z_scale = 1.0f; + + _mag_range_scale = MPU9250_MAG_RANGE_GA; +} + +ICM20948_mag::~ICM20948_mag() +{ + if (_mag_class_instance != -1) { + unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); + } + + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + orb_unadvertise(_mag_topic); + + perf_free(_mag_reads); + perf_free(_mag_errors); + perf_free(_mag_overruns); + perf_free(_mag_overflows); + perf_free(_mag_duplicates); +} + +int +ICM20948_mag::init() +{ + int ret = CDev::init(); + + /* if cdev init failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("ICM20948 mag init failed"); + + return ret; + } + + _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + + if (_mag_reports == nullptr) { + return -ENOMEM; + } + + _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag_orb_class_instance, (_parent->is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); +// &_mag_orb_class_instance, ORB_PRIO_LOW); + + if (_mag_topic == nullptr) { + PX4_ERR("ADVERT FAIL"); + return -ENOMEM; + } + + return OK; +} + +bool ICM20948_mag::check_duplicate(uint8_t *mag_data) +{ + if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) { + // it isn't new data - wait for next timer + return true; + + } else { + memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data)); + return false; + } +} + +void +ICM20948_mag::measure() +{ + uint8_t ret; + union raw_data_t { + struct ak8963_regs ak8963_data; + struct ak09916_regs ak09916_data; + } raw_data; + + + ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs)); + + if (ret == OK) { + raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; + + _measure(raw_data.ak8963_data); + } +} + +void +ICM20948_mag::_measure(struct ak8963_regs data) +{ + bool mag_notify = true; + + if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) { + perf_count(_mag_duplicates); + return; + } + + /* monitor for if data overrun flag is ever set */ + if (data.st1 & 0x02) { + perf_count(_mag_overruns); + } + + /* monitor for if magnetic sensor overflow flag is ever set noting that st2 + * is usually not even refreshed, but will always be in the same place in the + * mpu's buffers regardless, hence the actual count would be bogus + */ + if (data.st2 & 0x08) { + perf_count(_mag_overflows); + } + + mag_report mrb; + mrb.timestamp = hrt_absolute_time(); +// mrb.is_external = false; + + // need a better check here. Using _parent->is_external() for mpu9250 also sets the + // internal magnetometers connected to the "external" spi bus as external, at least + // on Pixhawk 2.1. For now assuming the ICM20948 is only used on Here GPS, hence external. + if (_parent->_whoami == ICM_WHOAMI_20948) { + mrb.is_external = _parent->is_external(); + + } else { + mrb.is_external = false; + } + + /* + * Align axes - note the accel & gryo are also re-aligned so this + * doesn't look obvious with the datasheet + */ + float xraw_f, yraw_f, zraw_f; + + if (_parent->_whoami == ICM_WHOAMI_20948) { + /* + * Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. + */ + mrb.x_raw = data.y; + mrb.y_raw = data.x; + mrb.z_raw = -data.z; + + xraw_f = data.y; + yraw_f = data.x; + zraw_f = -data.z; + + } else { + mrb.x_raw = data.x; + mrb.y_raw = -data.y; + mrb.z_raw = -data.z; + + xraw_f = data.x; + yraw_f = -data.y; + zraw_f = -data.z; + } + + /* apply user specified rotation */ + rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f); + + if (_parent->_whoami == ICM_WHOAMI_20948) { + rotate_3f(ROTATION_YAW_270, xraw_f, yraw_f, zraw_f); //offset between accel/gyro and mag on icm20948 + } + + mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale; + mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale; + mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale; + mrb.scaling = _mag_range_scale; + mrb.temperature = _parent->_last_temperature; + mrb.device_id = _parent->_mag->_device_id.devid; + + mrb.error_count = perf_event_count(_mag_errors); + + _mag_reports->force(&mrb); + + /* notify anyone waiting for data */ + if (mag_notify) { + poll_notify(POLLIN); + } + + if (mag_notify && !(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb); + } +} + +int +ICM20948_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + /* + * Repeated in ICM20948_accel::ioctl + * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 + */ + + switch (cmd) { + + case SENSORIOCRESET: + return ak8963_reset(); + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* zero would be bad */ + case 0: + return -EINVAL; + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: + return _parent->_set_pollrate(arg); + } + } + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + default: + return (int)CDev::ioctl(filp, cmd, arg); + } +} + +void +ICM20948_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) +{ + uint8_t addr; + + _parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers + + if (out) { + _parent->write_reg(ICMREG_20948_I2C_SLV0_DO, *out); + addr = AK8963_I2C_ADDR; + + } else { + addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG; + } + + _parent->write_reg(ICMREG_20948_I2C_SLV0_ADDR, addr); + _parent->write_reg(ICMREG_20948_I2C_SLV0_REG, reg); + _parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN); +} + +void +ICM20948_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count) +{ + _parent->_interface->read(reg, val, count); +} + +void +ICM20948_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) +{ + set_passthrough(reg, size); + px4_usleep(25 + 25 * size); // wait for the value to be read from slave + read_block(ICMREG_20948_EXT_SLV_SENS_DATA_00, buf, size); + _parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // disable new reads +} + +uint8_t +ICM20948_mag::read_reg(unsigned int reg) +{ + uint8_t buf; + + if (_interface == nullptr) { + passthrough_read(reg, &buf, 0x01); + + } else { + _interface->read(reg, &buf, 1); + } + + return buf; +} + +bool +ICM20948_mag::ak8963_check_id(uint8_t &deviceid) +{ + deviceid = read_reg(AK8963REG_WIA); + + return (AK8963_DEVICE_ID == deviceid); +} + +/* + * 400kHz I2C bus speed = 2.5us per bit = 25us per byte + */ +void +ICM20948_mag::passthrough_write(uint8_t reg, uint8_t val) +{ + set_passthrough(reg, 1, &val); + px4_usleep(50); // wait for the value to be written to slave + _parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // disable new writes +} + +void +ICM20948_mag::write_reg(unsigned reg, uint8_t value) +{ + // general register transfer at low clock speed + if (_interface == nullptr) { + passthrough_write(reg, value); + + } else { + _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); + } +} + +int +ICM20948_mag::ak8963_reset(void) +{ + // First initialize it to use the bus + int rv = ak8963_setup(); + + if (rv == OK) { + // Now reset the mag + write_reg(AK09916REG_CNTL3, AK8963_RESET); + + // Then re-initialize the bus/mag + rv = ak8963_setup(); + } + + return rv; +} + +bool +ICM20948_mag::ak8963_read_adjustments(void) +{ + uint8_t response[3]; + float ak8963_ASA[3]; + + write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC); + px4_usleep(50); + + if (_interface != nullptr) { + _interface->read(AK8963REG_ASAX, response, 3); + + } else { + passthrough_read(AK8963REG_ASAX, response, 3); + } + + write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); + + for (int i = 0; i < 3; i++) { + if (0 != response[i] && 0xff != response[i]) { + ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f; + + } else { + return false; + } + } + + _mag_asa_x = ak8963_ASA[0]; + _mag_asa_y = ak8963_ASA[1]; + _mag_asa_z = ak8963_ASA[2]; + + return true; +} + +int +ICM20948_mag::ak8963_setup_master_i2c(void) +{ + /* When _interface is null we are using SPI and must + * use the parent interface to configure the device to act + * in master mode (SPI to I2C bridge) + */ + if (_interface == nullptr) { + // ICM20948 -> AK09916 + _parent->modify_checked_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_EN); + + // WAIT_FOR_ES does not exist for ICM20948. Not sure how to replace this (or if that is needed) + _parent->write_reg(ICMREG_20948_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | ICM_BITS_I2C_MST_CLOCK_400HZ); + + } else { + _parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0); + } + + return OK; +} +int +ICM20948_mag::ak8963_setup(void) +{ + int retries = 10; + + do { + + ak8963_setup_master_i2c(); + write_reg(AK09916REG_CNTL3, AK8963_RESET); + + uint8_t id = 0; + + if (ak8963_check_id(id)) { + break; + } + + retries--; + PX4_WARN("AK8963: bad id %d retries %d", id, retries); + _parent->modify_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_RST); + up_udelay(100); + } while (retries > 0); + + if (retries == 0) { + PX4_ERR("AK8963: failed to initialize, disabled!"); + _parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0); + _parent->write_reg(ICMREG_20948_I2C_MST_CTRL, 0); + return -EIO; + } + + write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ); + + if (_interface == NULL) { + + /* Configure mpu' I2c Master interface to read ak8963 data + * Into to fifo + */ + + set_passthrough(AK09916REG_ST1, sizeof(struct ak09916_regs)); + } + + return OK; +} diff --git a/src/drivers/imu/icm20948/mag.h b/src/drivers/imu/icm20948/mag.h new file mode 100644 index 0000000000..6927bffe26 --- /dev/null +++ b/src/drivers/imu/icm20948/mag.h @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include "drivers/device/ringbuffer.h" // ringbuffer::RingBuffer +#include "drivers/drv_mag.h" // mag_calibration_s +#include + +/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ + +#define MPU9250_MAG_RANGE_GA 1.5e-3f; + +/* we are using the continuous fixed sampling rate of 100Hz */ + +#define MPU9250_AK8963_SAMPLE_RATE 100 + +/* ak8963 register address and bit definitions */ + +#define AK8963_I2C_ADDR 0x0C +#define AK8963_DEVICE_ID 0x48 + +#define AK8963REG_WIA 0x00 +#define AK8963REG_ST1 0x02 +#define AK8963REG_HXL 0x03 +#define AK8963REG_ASAX 0x10 +#define AK8963REG_CNTL1 0x0A +#define AK8963REG_CNTL2 0x0B + +#define AK8963_SINGLE_MEAS_MODE 0x01 +#define AK8963_CONTINUOUS_MODE1 0x02 +#define AK8963_CONTINUOUS_MODE2 0x06 +#define AK8963_POWERDOWN_MODE 0x00 +#define AK8963_SELFTEST_MODE 0x08 +#define AK8963_FUZE_MODE 0x0F +#define AK8963_16BIT_ADC 0x10 +#define AK8963_14BIT_ADC 0x00 +#define AK8963_RESET 0x01 +#define AK8963_HOFL 0x08 + +/* ak09916 deviating register addresses and bit definitions */ + +#define AK09916_DEVICE_ID_A 0x48 // same as AK8963 +#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.) + +#define AK09916REG_HXL 0x11 +#define AK09916REG_HXH 0x12 +#define AK09916REG_HYL 0x13 +#define AK09916REG_HYH 0x14 +#define AK09916REG_HZL 0x15 +#define AK09916REG_HZH 0x16 +#define AK09916REG_ST1 0x10 +#define AK09916REG_ST2 0x18 +#define AK09916REG_CNTL2 0x31 +#define AK09916REG_CNTL3 0x32 + + +#define AK09916_CNTL2_POWERDOWN_MODE 0x00 +#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */ +#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02 +#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04 +#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06 +#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08 +#define AK09916_CNTL2_SELFTEST_MODE 0x10 +#define AK09916_CNTL3_SRST 0x01 +#define AK09916_ST1_DRDY 0x01 +#define AK09916_ST1_DOR 0x02 + + +class ICM20948; + +#pragma pack(push, 1) +struct ak8963_regs { + uint8_t st1; + int16_t x; + int16_t y; + int16_t z; + uint8_t st2; +}; +#pragma pack(pop) + +#pragma pack(push, 1) +struct ak09916_regs { + uint8_t st1; + int16_t x; + int16_t y; + int16_t z; + uint8_t tmps; + uint8_t st2; +}; +#pragma pack(pop) + + +extern device::Device *AK8963_I2C_interface(int bus, bool external_bus); + +typedef device::Device *(*ICM20948_mag_constructor)(int, bool); + + +/** + * Helper class implementing the magnetometer driver node. + */ +class ICM20948_mag : public device::CDev +{ +public: + ICM20948_mag(ICM20948 *parent, device::Device *interface, const char *path); + ~ICM20948_mag(); + + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int init(); + + void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = NULL); + void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size); + void passthrough_write(uint8_t reg, uint8_t val); + void read_block(uint8_t reg, uint8_t *val, uint8_t count); + + int ak8963_reset(void); + int ak8963_setup(void); + int ak8963_setup_master_i2c(void); + bool ak8963_check_id(uint8_t &id); + bool ak8963_read_adjustments(void); + +protected: + Device *_interface; + + friend class ICM20948; + + /* Directly measure from the _interface if possible */ + void measure(); + + /* Update the state with prefetched data (internally called by the regular measure() )*/ + void _measure(struct ak8963_regs data); + + uint8_t read_reg(unsigned reg); + void write_reg(unsigned reg, uint8_t value); + + bool is_passthrough() { return _interface == nullptr; } + +private: + ICM20948 *_parent; + orb_advert_t _mag_topic; + int _mag_orb_class_instance; + int _mag_class_instance; + bool _mag_reading_data; + ringbuffer::RingBuffer *_mag_reports; + struct mag_calibration_s _mag_scale; + float _mag_range_scale; + perf_counter_t _mag_reads; + perf_counter_t _mag_errors; + perf_counter_t _mag_overruns; + perf_counter_t _mag_overflows; + perf_counter_t _mag_duplicates; + float _mag_asa_x; + float _mag_asa_y; + float _mag_asa_z; + + bool check_duplicate(uint8_t *mag_data); + + // keep last mag reading for duplicate detection + uint8_t _last_mag_data[6]; + + /* do not allow to copy this class due to pointer data members */ + ICM20948_mag(const ICM20948_mag &); + ICM20948_mag operator=(const ICM20948_mag &); +}; diff --git a/src/drivers/imu/icm20948/mag_i2c.cpp b/src/drivers/imu/icm20948/mag_i2c.cpp new file mode 100644 index 0000000000..b9f1b1e63b --- /dev/null +++ b/src/drivers/imu/icm20948/mag_i2c.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * 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 mag_i2c.cpp + * + * I2C interface for AK8963 + */ + +#include +#include +#include +#include + +#include "icm20948.h" +#include "mag.h" + +#ifdef USE_I2C + +device::Device *AK8963_I2C_interface(int bus, bool external_bus); + +class AK8963_I2C : public device::I2C +{ +public: + AK8963_I2C(int bus); + ~AK8963_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; + +}; + +device::Device * +AK8963_I2C_interface(int bus, bool external_bus) +{ + return new AK8963_I2C(bus); +} + +AK8963_I2C::AK8963_I2C(int bus) : + I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; +} + +int +AK8963_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] = MPU9250_REG(reg_speed); + cmd[1] = *(uint8_t *)data; + return transfer(&cmd[0], count + 1, nullptr, 0); +} + +int +AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count) +{ + uint8_t cmd = MPU9250_REG(reg_speed); + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int +AK8963_I2C::probe() +{ + uint8_t whoami = 0; + uint8_t expected = AK8963_DEVICE_ID; + + if (PX4_OK != read(AK8963REG_WIA, &whoami, 1)) { + return -EIO; + } + + if (whoami != expected) { + return -EIO; + } + + return OK; +} + +#endif diff --git a/src/drivers/imu/icm20948/main.cpp b/src/drivers/imu/icm20948/main.cpp new file mode 100644 index 0000000000..374439f37e --- /dev/null +++ b/src/drivers/imu/icm20948/main.cpp @@ -0,0 +1,429 @@ +/**************************************************************************** + * + * 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 main.cpp + * + * Driver for the Invensense icm20948 connected via I2C or SPI. + * + * based on the mpu9250 driver + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "icm20948.h" + +#define ICM_DEVICE_PATH_ACCEL_EXT "/dev/icm20948_accel_ext" +#define ICM_DEVICE_PATH_GYRO_EXT "/dev/icm20948_gyro_ext" +#define ICM_DEVICE_PATH_MAG_EXT "/dev/icm20948_mag_ext" + +/** driver 'main' command */ +extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); } + +enum ICM20948_BUS { + ICM20948_BUS_ALL = 0, + ICM20948_BUS_I2C_INTERNAL, + ICM20948_BUS_I2C_EXTERNAL, +// ICM20948_BUS_SPI_INTERNAL, +// ICM20948_BUS_SPI_INTERNAL2, + ICM20948_BUS_SPI_EXTERNAL +}; + +/** + * Local functions in support of the shell command. + */ +namespace icm20948 +{ + +/* + list of supported bus configurations + */ + +struct icm20948_bus_option { + enum ICM20948_BUS busid; + const char *accelpath; + const char *gyropath; + const char *magpath; + ICM20948_constructor interface_constructor; + bool magpassthrough; + uint8_t busnum; + uint32_t address; + ICM20948 *dev; +} bus_options[] = { +#if defined (USE_I2C) + +# if defined(PX4_I2C_BUS_EXPANSION) + { ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_ACCEL_EXT, ICM_DEVICE_PATH_GYRO_EXT, ICM_DEVICE_PATH_MAG_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, +#endif + +#endif + +}; + +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + + +void start(enum ICM20948_BUS busid, enum Rotation rotation, bool external_bus, bool magnetometer_only); +bool start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); +struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid); +void stop(enum ICM20948_BUS busid); +void reset(enum ICM20948_BUS busid); +void info(enum ICM20948_BUS busid); +void usage(); + +/** + * find a bus structure for a busid + */ +struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid) +{ + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((busid == ICM20948_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != nullptr) { + return bus_options[i]; + } + } + + errx(1, "bus %u not started", (unsigned)busid); +} + +/** + * start driver for a specific bus option + */ +bool +start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external, bool magnetometer_only) +{ + int fd = -1; + + PX4_INFO("Bus probed: %d", bus.busid); + + if (bus.dev != nullptr) { + warnx("%s SPI not available", external ? "External" : "Internal"); + return false; + } + + device::Device *interface = bus.interface_constructor(bus.busnum, bus.address, external); + + if (interface == nullptr) { + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + device::Device *mag_interface = nullptr; + +#ifdef USE_I2C + /* For i2c interfaces, connect to the magnetomer directly */ + bool is_i2c = bus.busid == ICM20948_BUS_I2C_INTERNAL || bus.busid == ICM20948_BUS_I2C_EXTERNAL; + + if (is_i2c) { + mag_interface = AK8963_I2C_interface(bus.busnum, external); + } + +#endif + + bus.dev = new ICM20948(interface, mag_interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, + magnetometer_only); + + if (bus.dev == nullptr) { + delete interface; + + if (mag_interface != nullptr) { + delete mag_interface; + } + + return false; + } + + if (OK != bus.dev->init()) { + goto fail; + } + + /* + * Set the poll rate to default, starts automatic data collection. + * Doing this through the mag device for the time being - it's always there, even in magnetometer only mode. + * Using accel device for MPU6500. + */ + fd = open(bus.magpath, O_RDONLY); + + if (fd < 0) { + PX4_INFO("ioctl failed"); + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + + close(fd); + + return true; + +fail: + + if (fd >= 0) { + close(fd); + } + + if (bus.dev != nullptr) { + delete (bus.dev); + bus.dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(enum ICM20948_BUS busid, enum Rotation rotation, bool external, bool magnetometer_only) +{ + + bool started = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (bus_options[i].dev != nullptr) { + // this device is already started + continue; + } + + if (busid != ICM20948_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i], rotation, external, magnetometer_only); + + if (started) { break; } + } + + exit(started ? 0 : 1); + +} + +void +stop(enum ICM20948_BUS busid) +{ + struct icm20948_bus_option &bus = find_bus(busid); + + + if (bus.dev != nullptr) { + delete bus.dev; + bus.dev = nullptr; + + } else { + /* warn, but not an error */ + warnx("already stopped."); + } + + exit(0); +} + +/** + * Reset the driver. + */ +void +reset(enum ICM20948_BUS busid) +{ + struct icm20948_bus_option &bus = find_bus(busid); + int fd = open(bus.accelpath, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + close(fd); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info(enum ICM20948_BUS busid) +{ + struct icm20948_bus_option &bus = find_bus(busid); + + + if (bus.dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", bus.dev); + bus.dev->print_info(); + + exit(0); +} + +void +usage() +{ + PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'"); + PX4_INFO("options:"); + PX4_INFO(" -X (i2c external bus)"); + PX4_INFO(" -I (i2c internal bus)"); + PX4_INFO(" -s (spi internal bus)"); + PX4_INFO(" -S (spi external bus)"); + PX4_INFO(" -t (spi internal bus, 2nd instance)"); + PX4_INFO(" -R rotation"); + PX4_INFO(" -M only enable magnetometer, accel/gyro disabled - not av. on MPU6500"); +} + +} // namespace icm20948 + +int +icm20948_main(int argc, char *argv[]) +{ + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + enum ICM20948_BUS busid = ICM20948_BUS_ALL; + enum Rotation rotation = ROTATION_NONE; + bool magnetometer_only = false; + + while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'X': + busid = ICM20948_BUS_I2C_EXTERNAL; + break; + +// case 'I': +// busid = ICM20948_BUS_I2C_INTERNAL; +// break; + +// case 'S': +// busid = ICM20948_BUS_SPI_EXTERNAL; +// break; +// +// case 's': +// busid = ICM20948_BUS_SPI_INTERNAL; +// break; +// +// case 't': +// busid = ICM20948_BUS_SPI_INTERNAL2; +// break; + + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + case 'M': + magnetometer_only = true; + break; + + default: + icm20948::usage(); + return 0; + } + } + + if (myoptind >= argc) { + icm20948::usage(); + return -1; + } + + bool external = busid == ICM20948_BUS_I2C_EXTERNAL || busid == ICM20948_BUS_SPI_EXTERNAL; + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + icm20948::start(busid, rotation, external, magnetometer_only); + } + + if (!strcmp(verb, "stop")) { + icm20948::stop(busid); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + icm20948::reset(busid); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + icm20948::info(busid); + } + + icm20948::usage(); + return 0; +} diff --git a/src/drivers/imu/mpu9250/mag.cpp b/src/drivers/imu/mpu9250/mag.cpp index dcf5aa9751..84130148e0 100644 --- a/src/drivers/imu/mpu9250/mag.cpp +++ b/src/drivers/imu/mpu9250/mag.cpp @@ -118,9 +118,9 @@ MPU9250_mag::init() /* if cdev init failed, bail now */ if (ret != OK) { - if (_parent->_whoami == MPU_WHOAMI_9250) { DEVICE_DEBUG("MPU9250 mag init failed"); } - - else { DEVICE_DEBUG("ICM20948 mag init failed"); } + if (_parent->_whoami == MPU_WHOAMI_9250) { + PX4_ERR("mag init failed"); + } return ret; } @@ -165,22 +165,14 @@ bool MPU9250_mag::check_duplicate(uint8_t *mag_data) void MPU9250_mag::measure() { - uint8_t ret; union raw_data_t { struct ak8963_regs ak8963_data; struct ak09916_regs ak09916_data; } raw_data; - if (_parent->_whoami == MPU_WHOAMI_9250) { - ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs)); - - } else { // ICM20948 --> AK09916 - ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs)); - } + uint8_t ret = _interface->read(AK8963REG_ST1, &raw_data, sizeof(struct ak8963_regs)); if (ret == OK) { - if (_parent->_whoami == ICM_WHOAMI_20948) { raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; } - _measure(raw_data.ak8963_data); } } @@ -210,17 +202,8 @@ MPU9250_mag::_measure(struct ak8963_regs data) mag_report mrb; mrb.timestamp = hrt_absolute_time(); -// mrb.is_external = false; - // need a better check here. Using _parent->is_external() for mpu9250 also sets the - // internal magnetometers connected to the "external" spi bus as external, at least - // on Pixhawk 2.1. For now assuming the ICM20948 is only used on Here GPS, hence external. - if (_parent->_whoami == ICM_WHOAMI_20948) { - mrb.is_external = _parent->is_external(); - - } else { - mrb.is_external = false; - } + mrb.is_external = _parent->is_external(); /* * Align axes - note the accel & gryo are also re-aligned so this @@ -228,35 +211,17 @@ MPU9250_mag::_measure(struct ak8963_regs data) */ float xraw_f, yraw_f, zraw_f; - if (_parent->_whoami == ICM_WHOAMI_20948) { - /* - * Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. - */ - mrb.x_raw = data.y; - mrb.y_raw = data.x; - mrb.z_raw = -data.z; + mrb.x_raw = data.x; + mrb.y_raw = -data.y; + mrb.z_raw = -data.z; - xraw_f = data.y; - yraw_f = data.x; - zraw_f = -data.z; - - } else { - mrb.x_raw = data.x; - mrb.y_raw = -data.y; - mrb.z_raw = -data.z; - - xraw_f = data.x; - yraw_f = -data.y; - zraw_f = -data.z; - } + xraw_f = data.x; + yraw_f = -data.y; + zraw_f = -data.z; /* apply user specified rotation */ rotate_3f(_parent->_rotation, xraw_f, yraw_f, zraw_f); - if (_parent->_whoami == ICM_WHOAMI_20948) { - rotate_3f(ROTATION_YAW_270, xraw_f, yraw_f, zraw_f); //offset between accel/gyro and mag on icm20948 - } - mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale; mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale; mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale; @@ -328,20 +293,19 @@ MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) { uint8_t addr; - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), - 0); // ensure slave r/w is disabled before changing the registers + _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers if (out) { - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_D0, ICMREG_20948_I2C_SLV0_DO), *out); + _parent->write_reg(MPUREG_I2C_SLV0_D0, *out); addr = AK8963_I2C_ADDR; } else { addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG; } - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_ADDR, ICMREG_20948_I2C_SLV0_ADDR), addr); - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_REG, ICMREG_20948_I2C_SLV0_REG), reg); - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), size | BIT_I2C_SLV0_EN); + _parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr); + _parent->write_reg(MPUREG_I2C_SLV0_REG, reg); + _parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN); } void @@ -355,8 +319,8 @@ MPU9250_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) { set_passthrough(reg, size); px4_usleep(25 + 25 * size); // wait for the value to be read from slave - read_block(AK_MPU_OR_ICM(MPUREG_EXT_SENS_DATA_00, ICMREG_20948_EXT_SLV_SENS_DATA_00), buf, size); - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), 0); // disable new reads + read_block(MPUREG_EXT_SENS_DATA_00, buf, size); + _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new reads } uint8_t @@ -390,7 +354,7 @@ MPU9250_mag::passthrough_write(uint8_t reg, uint8_t val) { set_passthrough(reg, 1, &val); px4_usleep(50); // wait for the value to be written to slave - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_SLV0_CTRL, ICMREG_20948_I2C_SLV0_CTRL), 0); // disable new writes + _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // disable new writes } void @@ -414,7 +378,7 @@ MPU9250_mag::ak8963_reset(void) if (rv == OK) { // Now reset the mag - write_reg(AK_MPU_OR_ICM(AK8963REG_CNTL2, AK09916REG_CNTL3), AK8963_RESET); + write_reg(AK8963REG_CNTL2, AK8963_RESET); // Then re-initialize the bus/mag rv = ak8963_setup(); } @@ -467,15 +431,10 @@ MPU9250_mag::ak8963_setup_master_i2c(void) if (_parent->_whoami == MPU_WHOAMI_9250) { _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_EN); _parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ); - - } else { // ICM20948 -> AK09916 - _parent->modify_checked_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_EN); - // WAIT_FOR_ES does not exist for ICM20948. Not sure how to replace this (or if that is needed) - _parent->write_reg(ICMREG_20948_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | ICM_BITS_I2C_MST_CLOCK_400HZ); } } else { - _parent->modify_checked_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), BIT_I2C_MST_EN, 0); + _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); } return OK; @@ -488,7 +447,7 @@ MPU9250_mag::ak8963_setup(void) do { ak8963_setup_master_i2c(); - write_reg(AK_MPU_OR_ICM(AK8963REG_CNTL2, AK09916REG_CNTL3), AK8963_RESET); + write_reg(AK8963REG_CNTL2, AK8963_RESET); uint8_t id = 0; @@ -498,42 +457,36 @@ MPU9250_mag::ak8963_setup(void) retries--; PX4_WARN("AK8963: bad id %d retries %d", id, retries); - _parent->modify_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), 0, BIT_I2C_MST_RST); + _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); up_udelay(100); } while (retries > 0); - /* No sensitivity adjustments available for AK09916/ICM20948 */ - if (_parent->_whoami == MPU_WHOAMI_9250) { - if (retries > 0) { - retries = 10; + if (retries > 0) { + retries = 10; - while (!ak8963_read_adjustments() && retries) { - retries--; - PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); + while (!ak8963_read_adjustments() && retries) { + retries--; + PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); - _parent->modify_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), 0, BIT_I2C_MST_RST); - up_udelay(100); - ak8963_setup_master_i2c(); - write_reg(AK_MPU_OR_ICM(AK8963REG_CNTL2, AK09916REG_CNTL3), AK8963_RESET); - } + _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); + up_udelay(100); + ak8963_setup_master_i2c(); + write_reg(AK8963REG_CNTL2, AK8963_RESET); } } if (retries == 0) { PX4_ERR("AK8963: failed to initialize, disabled!"); - _parent->modify_checked_reg(AK_MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), BIT_I2C_MST_EN, 0); - _parent->write_reg(AK_MPU_OR_ICM(MPUREG_I2C_MST_CTRL, ICMREG_20948_I2C_MST_CTRL), 0); + _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); + _parent->write_reg(MPUREG_I2C_MST_CTRL, 0); return -EIO; } if (_parent->_whoami == MPU_WHOAMI_9250) { write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); - } else { // ICM20948 -> AK09916 - write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ); } - if (_interface == NULL) { /* Configure mpu' I2c Master interface to read ak8963 data @@ -541,11 +494,7 @@ MPU9250_mag::ak8963_setup(void) */ if (_parent->_whoami == MPU_WHOAMI_9250) { set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); - - } else { // ICM20948 -> AK09916 - set_passthrough(AK09916REG_ST1, sizeof(struct ak09916_regs)); } - } return OK; diff --git a/src/drivers/imu/mpu9250/mag.h b/src/drivers/imu/mpu9250/mag.h index 32908c2e45..a4ad3dc713 100644 --- a/src/drivers/imu/mpu9250/mag.h +++ b/src/drivers/imu/mpu9250/mag.h @@ -96,11 +96,6 @@ #define AK09916_ST1_DRDY 0x01 #define AK09916_ST1_DOR 0x02 - -#define AK_MPU_OR_ICM(m,i) ((_parent->_whoami == MPU_WHOAMI_9250) ? (m) : (i)) - - - class MPU9250; #pragma pack(push, 1) diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index 7cd2845fd8..ed28431064 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -91,10 +91,6 @@ #define MPU_DEVICE_PATH_GYRO_EXT2 "/dev/mpu9250_gyro_ext2" #define MPU_DEVICE_PATH_MAG_EXT2 "/dev/mpu9250_mag_ext2" -#define ICM_DEVICE_PATH_ACCEL_EXT "/dev/icm20948_accel_ext" -#define ICM_DEVICE_PATH_GYRO_EXT "/dev/icm20948_gyro_ext" -#define ICM_DEVICE_PATH_MAG_EXT "/dev/icm20948_mag_ext" - /** driver 'main' command */ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } @@ -136,7 +132,6 @@ struct mpu9250_bus_option { # if defined(PX4_I2C_OBDEV_MPU9250) { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif - { MPU9250_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_ACCEL_EXT, ICM_DEVICE_PATH_GYRO_EXT, ICM_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, #endif # if defined(PX4_I2C_BUS_EXPANSION1) && defined(PX4_I2C_OBDEV_MPU9250) { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT1, MPU_DEVICE_PATH_GYRO_EXT1, MPU_DEVICE_PATH_MAG_EXT1, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr }, diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index eee5b57e52..14cfc35b27 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -89,26 +89,6 @@ const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS MPUREG_INT_PIN_CFG }; - -const uint16_t MPU9250::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { - ICMREG_20948_USER_CTRL, - ICMREG_20948_PWR_MGMT_1, - ICMREG_20948_PWR_MGMT_2, - ICMREG_20948_INT_PIN_CFG, - ICMREG_20948_INT_ENABLE, - ICMREG_20948_INT_ENABLE_1, - ICMREG_20948_INT_ENABLE_2, - ICMREG_20948_INT_ENABLE_3, - ICMREG_20948_GYRO_SMPLRT_DIV, - ICMREG_20948_GYRO_CONFIG_1, - ICMREG_20948_GYRO_CONFIG_2, - ICMREG_20948_ACCEL_SMPLRT_DIV_1, - ICMREG_20948_ACCEL_SMPLRT_DIV_2, - ICMREG_20948_ACCEL_CONFIG, - ICMREG_20948_ACCEL_CONFIG_2 -}; - - MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel, const char *path_gyro, const char *path_mag, enum Rotation rotation, @@ -138,8 +118,6 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _dlpf_freq_icm_gyro(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _dlpf_freq_icm_accel(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), _sample_rate(1000), _accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")), _gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")), @@ -360,7 +338,7 @@ MPU9250::init() } /* Magnetometer setup */ - if (_whoami == MPU_WHOAMI_9250 || _whoami == ICM_WHOAMI_20948) { + if (_whoami == MPU_WHOAMI_9250) { #ifdef USE_I2C @@ -448,7 +426,7 @@ int MPU9250::reset() ret = reset_mpu(); - if (ret == OK && (_whoami == MPU_WHOAMI_9250 || _whoami == ICM_WHOAMI_20948)) { + if (ret == OK && (_whoami == MPU_WHOAMI_9250)) { ret = _mag->ak8963_reset(); } @@ -472,22 +450,10 @@ int MPU9250::reset_mpu() write_checked_reg(MPUREG_PWR_MGMT_2, 0); usleep(1000); break; - - case ICM_WHOAMI_20948: - write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET); - usleep(1000); - - write_checked_reg(ICMREG_20948_PWR_MGMT_1, MPU_CLK_SEL_AUTO); - usleep(200); - write_checked_reg(ICMREG_20948_PWR_MGMT_2, 0); - break; } // Enable I2C bus or Disable I2C bus (recommended on data sheet) - - - write_checked_reg(MPU_OR_ICM(MPUREG_USER_CTRL, ICMREG_20948_USER_CTRL), is_i2c() ? 0 : BIT_I2C_IF_DIS); - + write_checked_reg(MPUREG_USER_CTRL, is_i2c() ? 0 : BIT_I2C_IF_DIS); // SAMPLE RATE _set_sample_rate(_sample_rate); @@ -500,10 +466,6 @@ int MPU9250::reset_mpu() case MPU_WHOAMI_6500: write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); break; - - case ICM_WHOAMI_20948: - modify_checked_reg(ICMREG_20948_GYRO_CONFIG_1, ICM_BITS_GYRO_FS_SEL_MASK, ICM_BITS_GYRO_FS_SEL_2000DPS); - break; } @@ -518,8 +480,7 @@ int MPU9250::reset_mpu() set_accel_range(ACCEL_RANGE_G); // INT CFG => Interrupt on Data Ready - write_checked_reg(MPU_OR_ICM(MPUREG_INT_ENABLE, ICMREG_20948_INT_ENABLE_1), - BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready #ifdef USE_I2C bool bypass = !_mag->is_passthrough(); @@ -536,11 +497,9 @@ int MPU9250::reset_mpu() * so bypass is true if the mag has an i2c non null interfaces. */ - write_checked_reg(MPU_OR_ICM(MPUREG_INT_PIN_CFG, ICMREG_20948_INT_PIN_CFG), - BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); - write_checked_reg(MPU_OR_ICM(MPUREG_ACCEL_CONFIG2, ICMREG_20948_ACCEL_CONFIG_2), - MPU_OR_ICM(BITS_ACCEL_CONFIG2_41HZ, ICM_BITS_DEC3_CFG_32)); + write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); retries = 3; bool all_ok = false; @@ -554,9 +513,6 @@ int MPU9250::reset_mpu() for (uint8_t i = 0; i < _num_checked_registers; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { - if (_whoami == ICM_WHOAMI_20948) { - _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1); - } write_reg(_checked_registers[i], _checked_values[i]); PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, @@ -577,13 +533,6 @@ MPU9250::probe() // Try first for mpu9250/6500 _whoami = read_reg(MPUREG_WHOAMI); - // If it's not an MPU it must be an ICM - if ((_whoami != MPU_WHOAMI_9250) && (_whoami != MPU_WHOAMI_6500)) { - // Make sure selected register bank is bank 0 (which contains WHOAMI) - select_register_bank(REG_BANK(ICMREG_20948_WHOAMI)); - _whoami = read_reg(ICMREG_20948_WHOAMI); - } - if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { _num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS; @@ -591,14 +540,6 @@ MPU9250::probe() memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS); memset(_checked_bad, 0, MPU9250_NUM_CHECKED_REGISTERS); ret = PX4_OK; - - } else if (_whoami == ICM_WHOAMI_20948) { - - _num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS; - _checked_registers = _icm20948_checked_registers; - memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS); - memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS); - ret = PX4_OK; } _checked_values[0] = _whoami; @@ -613,7 +554,6 @@ MPU9250::probe() /* set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro - For ICM20948 accel and gyro samplerates are both set to the same value. */ void MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) @@ -629,10 +569,6 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) case MPU_WHOAMI_6500: div = 1000 / desired_sample_rate_hz; break; - - case ICM_WHOAMI_20948: - div = 1100 / desired_sample_rate_hz; - break; } if (div > 200) { div = 200; } @@ -646,14 +582,6 @@ MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; break; - - case ICM_WHOAMI_20948: - write_checked_reg(ICMREG_20948_GYRO_SMPLRT_DIV, div - 1); - // There's also an MSB for this allowing much higher dividers for the accelerometer. - // For 1 < div < 200 the LSB is sufficient. - write_checked_reg(ICMREG_20948_ACCEL_SMPLRT_DIV_2, div - 1); - _sample_rate = 1100 / div; - break; } } @@ -766,151 +694,15 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) write_checked_reg(MPUREG_CONFIG, filter); break; - - case ICM_WHOAMI_20948: - - /* - choose next highest filter frequency available for gyroscope - */ - if (frequency_hz == 0) { - _dlpf_freq_icm_gyro = 0; - filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; - - } else if (frequency_hz <= 5) { - _dlpf_freq_icm_gyro = 5; - filter = ICM_BITS_GYRO_DLPF_CFG_5HZ; - - } else if (frequency_hz <= 11) { - _dlpf_freq_icm_gyro = 11; - filter = ICM_BITS_GYRO_DLPF_CFG_11HZ; - - } else if (frequency_hz <= 23) { - _dlpf_freq_icm_gyro = 23; - filter = ICM_BITS_GYRO_DLPF_CFG_23HZ; - - } else if (frequency_hz <= 51) { - _dlpf_freq_icm_gyro = 51; - filter = ICM_BITS_GYRO_DLPF_CFG_51HZ; - - } else if (frequency_hz <= 119) { - _dlpf_freq_icm_gyro = 119; - filter = ICM_BITS_GYRO_DLPF_CFG_119HZ; - - } else if (frequency_hz <= 151) { - _dlpf_freq_icm_gyro = 151; - filter = ICM_BITS_GYRO_DLPF_CFG_151HZ; - - } else if (frequency_hz <= 197) { - _dlpf_freq_icm_gyro = 197; - filter = ICM_BITS_GYRO_DLPF_CFG_197HZ; - - } else { - _dlpf_freq_icm_gyro = 0; - filter = ICM_BITS_GYRO_DLPF_CFG_361HZ; - } - - write_checked_reg(ICMREG_20948_GYRO_CONFIG_1, filter); - - /* - choose next highest filter frequency available for accelerometer - */ - if (frequency_hz == 0) { - _dlpf_freq_icm_accel = 0; - filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; - - } else if (frequency_hz <= 5) { - _dlpf_freq_icm_accel = 5; - filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ; - - } else if (frequency_hz <= 11) { - _dlpf_freq_icm_accel = 11; - filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ; - - } else if (frequency_hz <= 23) { - _dlpf_freq_icm_accel = 23; - filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ; - - } else if (frequency_hz <= 50) { - _dlpf_freq_icm_accel = 50; - filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ; - - } else if (frequency_hz <= 111) { - _dlpf_freq_icm_accel = 111; - filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ; - - } else if (frequency_hz <= 246) { - _dlpf_freq_icm_accel = 246; - filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ; - - } else { - _dlpf_freq_icm_accel = 0; - filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ; - } - - write_checked_reg(ICMREG_20948_ACCEL_CONFIG, filter); - break; - } -} - -int -MPU9250::select_register_bank(uint8_t bank) -{ - uint8_t ret; - uint8_t buf; - uint8_t retries = 3; - - if (_selected_bank != bank) { - ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); - - if (ret != OK) { - return ret; - } - } - - /* - * Making sure the right register bank is selected (even if it should be). Observed some - * unexpected changes to this, don't risk writing to the wrong register bank. - */ - _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); - - while (bank != buf && retries > 0) { - //PX4_WARN("user bank: expected %d got %d",bank,buf); - ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); - - if (ret != OK) { - return ret; - } - - retries--; - //PX4_WARN("BANK retries: %d", 4-retries); - - _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); - } - - - _selected_bank = bank; - - if (bank != buf) { - PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf); - return PX4_ERROR; - - } else { - return PX4_OK; } } uint8_t MPU9250::read_reg(unsigned reg, uint32_t speed) { - uint8_t buf; + uint8_t buf{}; - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(reg)); - _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); - - } else { - _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); - } + _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); return buf; } @@ -918,35 +710,20 @@ MPU9250::read_reg(unsigned reg, uint32_t speed) uint8_t MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) { - uint8_t ret; - - if (buf == NULL) { return PX4_ERROR; } - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(start_reg)); - ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); - - } else { - ret = _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + if (buf == NULL) { + return PX4_ERROR; } - return ret; + return _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); } uint16_t MPU9250::read_reg16(unsigned reg) { - uint8_t buf[2]; + uint8_t buf[2] {}; // general register transfer at low clock speed - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(reg)); - _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); - - } else { - _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); - } + _interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf)); return (uint16_t)(buf[0] << 8) | buf[1]; } @@ -955,14 +732,7 @@ void MPU9250::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed - - if (_whoami == ICM_WHOAMI_20948) { - select_register_bank(REG_BANK(reg)); - _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); - - } else { - _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); - } + _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); } void @@ -1034,10 +804,6 @@ MPU9250::set_accel_range(unsigned max_g_in) case MPU_WHOAMI_6500: write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); break; - - case ICM_WHOAMI_20948: - modify_checked_reg(ICMREG_20948_ACCEL_CONFIG, ICM_BITS_ACCEL_FS_SEL_MASK, afs_sel << 1); - break; } _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); @@ -1169,12 +935,8 @@ MPU9250::check_registers(void) // if the product_id is wrong then reset the // sensor completely - if (_whoami == ICM_WHOAMI_20948) { - // reset_mpu(); - } else { - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); - } + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); // after doing a reset we need to wait a long // time before we do any other register writes @@ -1253,8 +1015,6 @@ MPU9250::measure() struct MPUReport mpu_report; - struct ICMReport icm_report; - struct Report { int16_t accel_x; int16_t accel_y; @@ -1279,19 +1039,11 @@ MPU9250::measure() return; } - } else { // ICM20948 - select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); - - if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report, - sizeof(icm_report))) { - perf_end(_sample_perf); - return; - } } check_registers(); - if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) { + if (check_duplicate(&mpu_report.accel_x[0])) { return; } } @@ -1324,24 +1076,13 @@ MPU9250::measure() /* * Convert from big to little endian */ - if (_whoami == ICM_WHOAMI_20948) { - report.accel_x = int16_t_from_bytes(icm_report.accel_x); - report.accel_y = int16_t_from_bytes(icm_report.accel_y); - report.accel_z = int16_t_from_bytes(icm_report.accel_z); - report.temp = int16_t_from_bytes(icm_report.temp); - report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); - report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); - report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); - - } else { // MPU9250/MPU6500 - 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); - } + 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 (check_null_data((uint16_t *)&report, sizeof(report) / 2)) { return; @@ -1371,27 +1112,22 @@ MPU9250::measure() if (!_magnetometer_only) { /* - * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation + * Swap axes and negate y */ - if (_whoami != ICM_WHOAMI_20948) { - /* - * 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 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); + 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; - } + /* + * Apply the swap + */ + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; /* * Report buffers. diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 2c540aa5b2..3d25e52879 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -183,7 +183,6 @@ #define MPU_WHOAMI_9250 0x71 #define MPU_WHOAMI_6500 0x70 -#define ICM_WHOAMI_20948 0xEA #define MPU9250_ACCEL_DEFAULT_RATE 1000 #define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 @@ -195,26 +194,6 @@ #define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92 -#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) - - - -// ICM20948 registers and data - -/* - * ICM20948 I2C address LSB can be switched by the chip's AD0 pin, thus is device dependent. - * Noting this down for now. Here GPS uses 0x69. To support a device implementing the second - * address, probably an additional MPU_DEVICE_TYPE is the way to go. - */ -#define PX4_I2C_EXT_ICM20948_0 0x68 -#define PX4_I2C_EXT_ICM20948_1 0x69 - -/* - * ICM20948 uses register banks. Register 127 (0x7F) is used to switch between 4 banks. - * There's room in the upper address byte below the port speed setting to code in the - * used bank. This is a bit more efficient, already in use for the speed setting and more - * in one place than a solution with a lookup table for address/bank pairs. - */ #define BANK0 0x0000 #define BANK1 0x0100 @@ -225,108 +204,6 @@ #define REG_BANK(r) (((r) & BANK_REG_MASK)>>4) #define REG_ADDRESS(r) ((r) & ~BANK_REG_MASK) -#define ICMREG_20948_BANK_SEL 0x7F - -#define ICMREG_20948_WHOAMI (0x00 | BANK0) -#define ICMREG_20948_USER_CTRL (0x03 | BANK0) -#define ICMREG_20948_PWR_MGMT_1 (0x06 | BANK0) -#define ICMREG_20948_PWR_MGMT_2 (0x07 | BANK0) -#define ICMREG_20948_INT_PIN_CFG (0x0F | BANK0) -#define ICMREG_20948_INT_ENABLE (0x10 | BANK0) -#define ICMREG_20948_INT_ENABLE_1 (0x11 | BANK0) -#define ICMREG_20948_ACCEL_XOUT_H (0x2D | BANK0) -#define ICMREG_20948_INT_ENABLE_2 (0x12 | BANK0) -#define ICMREG_20948_INT_ENABLE_3 (0x13 | BANK0) -#define ICMREG_20948_EXT_SLV_SENS_DATA_00 (0x3B | BANK0) -#define ICMREG_20948_GYRO_SMPLRT_DIV (0x00 | BANK2) -#define ICMREG_20948_GYRO_CONFIG_1 (0x01 | BANK2) -#define ICMREG_20948_GYRO_CONFIG_2 (0x02 | BANK2) -#define ICMREG_20948_ACCEL_SMPLRT_DIV_1 (0x10 | BANK2) -#define ICMREG_20948_ACCEL_SMPLRT_DIV_2 (0x11 | BANK2) -#define ICMREG_20948_ACCEL_CONFIG (0x14 | BANK2) -#define ICMREG_20948_ACCEL_CONFIG_2 (0x15 | BANK2) -#define ICMREG_20948_I2C_MST_CTRL (0x01 | BANK3) -#define ICMREG_20948_I2C_SLV0_ADDR (0x03 | BANK3) -#define ICMREG_20948_I2C_SLV0_REG (0x04 | BANK3) -#define ICMREG_20948_I2C_SLV0_CTRL (0x05 | BANK3) -#define ICMREG_20948_I2C_SLV0_DO (0x06 | BANK3) - - - -/* -* ICM20948 register bits -* Most of the regiser set values from MPU9250 have the same -* meaning on ICM20948. The exceptions and values not already -* defined for MPU9250 are defined below -*/ -#define ICM_BIT_PWR_MGMT_1_ENABLE 0x00 -#define ICM_BIT_USER_CTRL_I2C_MST_DISABLE 0x00 - -#define ICM_BITS_GYRO_DLPF_CFG_197HZ 0x01 -#define ICM_BITS_GYRO_DLPF_CFG_151HZ 0x09 -#define ICM_BITS_GYRO_DLPF_CFG_119HZ 0x11 -#define ICM_BITS_GYRO_DLPF_CFG_51HZ 0x19 -#define ICM_BITS_GYRO_DLPF_CFG_23HZ 0x21 -#define ICM_BITS_GYRO_DLPF_CFG_11HZ 0x29 -#define ICM_BITS_GYRO_DLPF_CFG_5HZ 0x31 -#define ICM_BITS_GYRO_DLPF_CFG_361HZ 0x39 -#define ICM_BITS_GYRO_DLPF_CFG_MASK 0x39 - -#define ICM_BITS_GYRO_FS_SEL_250DPS 0x00 -#define ICM_BITS_GYRO_FS_SEL_500DPS 0x02 -#define ICM_BITS_GYRO_FS_SEL_1000DPS 0x04 -#define ICM_BITS_GYRO_FS_SEL_2000DPS 0x06 -#define ICM_BITS_GYRO_FS_SEL_MASK 0x06 - -#define ICM_BITS_ACCEL_DLPF_CFG_246HZ 0x09 -#define ICM_BITS_ACCEL_DLPF_CFG_111HZ 0x11 -#define ICM_BITS_ACCEL_DLPF_CFG_50HZ 0x19 -#define ICM_BITS_ACCEL_DLPF_CFG_23HZ 0x21 -#define ICM_BITS_ACCEL_DLPF_CFG_11HZ 0x29 -#define ICM_BITS_ACCEL_DLPF_CFG_5HZ 0x31 -#define ICM_BITS_ACCEL_DLPF_CFG_473HZ 0x39 -#define ICM_BITS_ACCEL_DLPF_CFG_MASK 0x39 - -#define ICM_BITS_ACCEL_FS_SEL_250DPS 0x00 -#define ICM_BITS_ACCEL_FS_SEL_500DPS 0x02 -#define ICM_BITS_ACCEL_FS_SEL_1000DPS 0x04 -#define ICM_BITS_ACCEL_FS_SEL_2000DPS 0x06 -#define ICM_BITS_ACCEL_FS_SEL_MASK 0x06 - -#define ICM_BITS_DEC3_CFG_4 0x00 -#define ICM_BITS_DEC3_CFG_8 0x01 -#define ICM_BITS_DEC3_CFG_16 0x10 -#define ICM_BITS_DEC3_CFG_32 0x11 -#define ICM_BITS_DEC3_CFG_MASK 0x11 - -#define ICM_BITS_I2C_MST_CLOCK_370KHZ 0x00 -#define ICM_BITS_I2C_MST_CLOCK_400HZ 0x07 // recommended by datasheet for 400kHz target clock - - - -#define MPU_OR_ICM(m,i) ((_whoami==ICM_WHOAMI_20948) ? i : m) - - -#pragma pack(push, 1) -/** - * Report conversation within the mpu, including command byte and - * interrupt status. - */ -struct ICMReport { - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - uint8_t temp[2]; - struct ak8963_regs mag; -}; -#pragma pack(pop) - - - - #pragma pack(push, 1) /** * Report conversation within the mpu, including command byte and @@ -438,8 +315,6 @@ private: float _gyro_range_rad_s; unsigned _dlpf_freq; - unsigned _dlpf_freq_icm_gyro; - unsigned _dlpf_freq_icm_accel; unsigned _sample_rate; perf_counter_t _accel_reads; @@ -474,15 +349,13 @@ private: #define MAX(X,Y) ((X) > (Y) ? (X) : (Y)) #endif -#define MPU9250_NUM_CHECKED_REGISTERS 11 + static constexpr int MPU9250_NUM_CHECKED_REGISTERS{11}; static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; -#define ICM20948_NUM_CHECKED_REGISTERS 15 - static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS]; const uint16_t *_checked_registers; - uint8_t _checked_values[MAX(MPU9250_NUM_CHECKED_REGISTERS, ICM20948_NUM_CHECKED_REGISTERS)]; - uint8_t _checked_bad[MAX(MPU9250_NUM_CHECKED_REGISTERS, ICM20948_NUM_CHECKED_REGISTERS)]; + uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS]; + uint8_t _checked_bad[MPU9250_NUM_CHECKED_REGISTERS]; unsigned _checked_next; unsigned _num_checked_registers; @@ -570,18 +443,6 @@ private: */ void measure(); - /** - * Select a register bank in ICM20948 - * - * Only actually switches if the remembered bank is different from the - * requested one - * - * @param The index of the register bank to switch to (0-3) - * @return Error code - */ - int select_register_bank(uint8_t bank); - - /** * Read a register from the mpu * diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index 42c4188c9c..8a856f9ce1 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -108,26 +108,12 @@ int MPU9250_I2C::probe() { uint8_t whoami = 0; - uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948 // Try first for mpu9250/6500 read(MPUREG_WHOAMI, &whoami, 1); if (whoami == MPU_WHOAMI_9250 || whoami == MPU_WHOAMI_6500) { return PX4_OK; - - } else { - /* - * If it's not an MPU it must be an ICM - * Make sure register bank 0 is selected - whoami is only present on bank 0, and that is - * not sure e.g. if the device has rebooted without repowering the sensor - */ - write(ICMREG_20948_BANK_SEL, ®ister_select, 1); - read(ICMREG_20948_WHOAMI, &whoami, 1); - - if (whoami == ICM_WHOAMI_20948) { - return PX4_OK; - } } return -ENODEV;